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A  FORTRAN  IV  program  package  has  been  written  for  the  generation, 
analysis,  and  estimation  of  re-entry  trajectories.  The  following  func¬ 
tions  may  be  performed  by  the  program: 

(1)  Generation  of  simulated  noiseless  trajectory  data  by 
integrating  the  differential  equations  of  motion,  using 
a  predictor-corrector  method. 

(2)  Error  analyses  on  trajectories  generated  by  the  pro¬ 
gram,  in  which  the  effects  of  hypothetical  errors  in 
the  initial  state  and  subsequent  measurements  may 
be  studied. 

(3)  Estimation  of  state  vector  quantities,  using  a  re¬ 
cursive  Kalman-filter  scheme,  from 

(a)  Simulated  noisy  data  generated  by  the  program. 

(b)  Noisy  radar  measurements,  supplied  externally 
to  the  program. 


Accepted  for  the  Air  Force 

Franklin  C.  Hudson 

Chief,  Lincoln  Laboratory  Office 
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A  TRAJECTORY  ANALYSIS  PROGRAM  (TRAP) 


I.  INTRODUCTION 

A  FORTRAN  IV  double-precision  program  package  has  been  written  for  the  generation, 
analysis,  and  estimation  of  re-entry  trajectories.  The  trajectories  are  characterized  by  a 
seven-component  state  vector  specifying  position,  first  time  derivative  of  position,  and  drag 
parameter  defined  as  the  reciprocal  of  the  weight-to-drag  ratio.  The  program  assumes  a  spher 
ical,  rotating  earth  with  an  atmosphere  and  uses  either  English  or  metric  units.  The  following 
functions  may  be  performed  by  the  program  package: 

(a)  Generation  of  simulated  noiseless  trajectory  data  by  integrating 
the  differential  equations  of  motion,  using  a  predictor-corrector 
method. 

(b)  Error  analyses  on  trajectories  generated  by  the  program,  in  which 
the  effects  of  hypothetical  errors  in  the  initial  state  and  subsequent 
measurements  may  be  studied. 

(c)  Estimation  of  state  vector  quantities,  using  a  recursive  Kalman- 
filter  scheme,  from 

(1)  Simulated  noisy  data  generated  by  the  program. 

(2)  Noisy  radar  measurements,  supplied  externally 
to  the  program. 

A  modified  version  of  the  main  program  is  required  to  perform  function  (c)  (2). 

The  state  vector  which  describes  the  trajectory  motion  has  seven  components,  and  may  be 
considered  in  two  coordinate  systems.  In  radar-centered  rectangular  coordinates,  with  x  point 
id  z  vertically  upward,  we  have  the  following 

x 

y 

z 

X 

y 

z 

o  =  drag  parameter  =  l//? 

In  radar-centered  polar  coordinates,  which  is  the  usual  measurement  coordinate  system 
r^  =  range 
-  azimuth 
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Input  and  output  quantities  may  be  given  in  polar  or  rectangular  coordinates  but  all  calcula¬ 
tions  are  done  in  rectangular  coordinates.  English  (pounds,  feet,  seconds)  or  metric  (meters, 
kilograms,  seconds)  units  may  be  specified. 

The  program  package  consists  of  the  main  program  plus  ten  subroutines.  The  subroutines 
perform  various  calculations,  ranging  from  integrating  the  differential  equations  of  motion  to 
multiplying  matrices.  Some  of  these  subroutines  may  be  of  general  interest  for  use  outside  of 
the  program  package.  The  functions  of  the  main  program  and  all  the  subroutmes  are  summar¬ 
ized  in  Table  I,  with  full  details  given  in  later  sections. 

II.  MAIN  PROGRAM  (VERSION  I) 

The  main  program  reads  input  cards,  calls  the  appropriate  subroutines  to  perform  the  de¬ 
sired  calculations,  and  prints  output  data.  Data  on  the  input  cards  completely  specifies  the 
program  function  (e.  g.,  trajectory  generation,  error  analysis,  or  estimation),  furnishes  re¬ 
quired  initial  conditions,  and  specifies  the  amount  of  printed  output  desired.  If  required,  the 
user  may  change  the  program  so  that  the  output  data  may  be  written  on  tape,  plotted,  or  punched 
on  cards.  The  formats  of  the  seven  required  input  cards  are  given  in  Table  II.  Any  number  of 
trajectories  may  be  run  in  succession  by  stacking  the  input  cards  for  each  case  in  succession. 

A  block  diagram  of  the  main  program  is  shown  in  Fig.  1. 

The  following  COMMON  statements  are  used: 

COMMON /ACOM/COVAR(7),  SIGMA(7 )/FCOM/COORD,  DLAT, 
PRNT/ICOM/KLAMP,  MDATA,  NN 

An  explanation  of  these  variables  is  given  in  Tabic  II. 

Varying  amounts  of  printed  output  may  be  obtained  by  proper  specification  of  the  print- 
selector  parameter  PRNT.  These  are  summarized  below. 

PRNT  <  0.  Every  50  data  points,  the  following  tabulations  are 
printed  for  the  preceding  50  data  points. 

(a)  Time,  height,  and  the  seven  components  of  the 
nominal  state  vector  in  radar-centcred  polar 
coordinates. 

(b)  Time,  aspect  angle,  and  the  seven  components 
of  the  nominal  state  vector  in  radar-centered 
rectangular  coordinates. 

(c)  Time,  plus  the  measurement  vector  in  radar- 
centered  polar  coordinates  (printed  only  in 
estimation  cases). 

(d)  Time,  and  the  estimated  state  vector  in  radar- 
centered  polar  and  rectangular  coordinates 
(printed  only  in  estimation  cases). 

(e)  Time,  and  the  errors  (difference  between 
estimated  and  nominal  state  vector)  in  polar 
and  rectangular  coordinates. 

(f)  Theoretical  rms  errors,  obtained  from  co- 
variance  matrices,  in  polar  and  rectangular 
coordinates. 

Note:  The  above  may  be  printed  out  at  data  intervals 
other  than  every  50  points  by  properly  specifying  the 
value  of  IMAX  in  statement  9  of  the  main  program 
(see  Tables  B-I  and  B-II  in  Appendix  B).  All  one- 
dimensioned  arrays  of  size  54  should  be  redimensioned 
to  be  equal  to  or  greater  than  the  new  value  of  IMAX. 
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Fig.  1.  Flow  chart  of  TRAP  (version  I)  main  program. 
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Fig.  1.  Continued. 
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TABLE  I 

LIST  OF  PROGRAMS  IN  TRAP  PACKAGE 

Program  or  Subroutine 

Function 

Main  Program 
(Versions  I  and  II) 

Reads  input  cards  specifying  desired  calculations  and 
initial  conditions,  calls  appropriate  subroutines,  and 
prints  out  results. 

TRAJGX 

Integrates  differential  equations  of  motion  in  radar- 
centered  rectangular  coordinates.  A  spherical  rotating 
earth  with  a  realistic  atmosphere  is  used. 

ESTMAT 

Estimates  the  true  value  of  a  state  vector  by  combining  a 
predicted  value  of  the  state  vector  with  a  noisy  measure¬ 
ment.  Also  calculates  covariance  matrices  for  error 
analyses. 

DENS 

Calculates  atmospheric  density  at  any  given  altitude. 

XYTOR 

Converts  from  radar-centered  rectangular  coordinates 
to  polar  coordinates. 

RTOXY 

Converts  from  radar-centered  polar  coordinates  to  rec¬ 
tangular  coordinates. 

XCOVTR 

Converts  a  mean-square  error  covariance  matrix  in  rec¬ 
tangular  coordinates  to  one  in  polar  coordinates.  It  also 
calculates  a  partial  derivative  matrix  H  =  0r/3x  where 
jr  and  x  are  the  state  vector  in  radar-centered  polar  and 
rectangular  coordinates  respectively. 

RCOVTX 

Converts  a  mean- square  error  covariance  matrix  in  po¬ 
lar  coordinates  to  one  in  rectangular  coordinates. 

GAUSSN 

Generates  random  numbers  with  a  Gaussian  distribution. 

DMTMUL 

Multiplies  two  matrices  in  double  precision. 

MINV 

Matrix  inversion  subroutine  from  IBM  360  Scientific 
Subroutine  Package  (double-precision  version  used). 

PRNT  -  0.  All  of  the  above  are  printed,  as  well  as  the  following 

which  are  printed  for  every  data  point. 

(a)  Time,  height,  aspect  angle,  and  the  nominal  state 
vector  in  both  polar  and  rectangular  coordinates. 

(b)  The  transition  matrix,  9x(t^)/9x(tk-l )  (see  discussion 
of  subroutine  TRAJGX,  Sec.  IV),  along  the  nominal 
trajectory  in  rectangular  coordinates. 

(c)  The  noisy  measurement  vector,  in  polar  coordinates 
(printed  only  in  estimation  cases). 

(d)  The  predicted  state  vector,  in  polar  and  rectangular 
coordinates  (printed  only  in  estimation  cases). 

(e)  The  covariance  matrices  of  the  estimated  state  vector 
(see  discussion  of  subroutine  ESTMAT,  Sec.  V). 

(f)  The  estimated  state  vector  in  polar  and  rectangular 
coordinates. 

PRNT  >  0.  All  of  the  above  are  printed,  as  well  as  the  following  which 

are  printed  by  subroutine  ESTMAT  for  every  data  point. 

(a)  Transition  matrix  used  in  the  estimation. 

(b)  Covariance  matrices  for  predicted  and  estimated 
data  points. 

(c)  Various  vectors  and  matrices  used  in  the  calculation 
of  the  final  covariance  matrices  and  the  estimate  (see 
description  of  subroutine  ESTMAT,  Sec.  V). 
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TABLE  II 

INPUT  CARDS  TO  MAIN  PROGRAM  (VERSION  I) 

Card 

Format 

Arguments  and/or  Description 

1 

1 8A4 

Title  card  with  72  alpha-numerieal  characters  available. 

2 

7F10.3 

(STATEI  ( J ),  J  =  1,  7)  where  STATEI  is  initial  value  of  nominal 
state  vector,  in  either  polar  or  rectangular  radar-eentered 
coordinates. 

3 

5F10.3,  F5. 3, 
315 

TZERO  =  initial  time  (in  seeonds) 

DELT  =  time  of  first  measurement  with  respect  to  time  of 
initial  state 

TINT  =  interval,  in  seeonds,  between  data  points 

TINCR  =  trajectory  integration  step  size,  in  seconds 

DLAT  =  latitude  of  radar  (reference)  site  in  degrees 

PRNT  =  print-seleetor  parameter  (see  text,  p.  2) 

NDATA  =  number  of  data  points  to  be  processed 

KLAMP  =  "memory"  of  estimation  algorithm,  given  in  number 
of  data  points  (see  description  of  ESTMAT,  See.  V); 
KLAMP  =  0  is  used  to  indicate  infinite  memory 

MDATA  =  mode  parameter 

MDATA  <  0  specifies  error  analysis 

MDATA  -  0  specifies  noiseless  trajectory  generation 
MDATA  >  0  specifies  estimation 

The  magnitude  of  MDATA  <  7  and  specifies  the  size 
of  the  measurement  vector  used  in  calculations  (see 
description  of  ESTMAT,  See.  V). 

4 

7F10.3 

(SIGMA(J),  J  =  1,6)  rms  noise  levels  associated  with  meas¬ 
urement  vector.  No  value  is  given  to  SIGMA(7)  since  the  drag 
parameter,  whieh  is  the  seventh  component  of  the  state  vector, 
is  not  measurable  directly. 

COORD  =  Coordinate  —  specification  parameter 

COORD  =  —  2.  Initial  state  veetor  given  in  radar- 
eentered  polar  coordinates.  Metric 
units  used  for  all  calculations. 

COORD  =  —  1.  Initial  state  veetor  given  in  radar- 
eentered  rectangular  coordinates. 

Metric  units  used  for  all  calculations. 

COORD  =  1.  Initial  state  veetor  given  in  radar- 

eentered  rectangular  coordinates. 

English  units  used  for  all  calculations. 

COORD  =  2.  Initial  state  veetor  given  in  radar- 

eentered  polar  coordinates.  English 
units  used  for  all  calculations. 

5 

7F10.3 

(COVAR(J),  J  =  1,7)  =  rms  noise  levels  associated  with  initial 
state  veetor  in  units  specified  by  the  parameter  COORD. 

6 

7F10.3 

(FNOISE(J),  J  1,7)=  noise  samples  associated  with  initial 
state  veetor,  in  units  specified  by  the  parameter  COORD. 

7 

7 FI  0.  3 

(ERRMAX(J),  J  -  1,7)  -  magnitude  of  maximum  allowable  con¬ 
vergence  errors.  The  convergence  errors  are  an  indication  of 
the  aeeuraey  of  the  integration  of  the  trajectory  equations  —  the 
smaller  the  errors,  the  better  the  aeeuraey.  Values  of  0.1  for 
all  seven  elements  in  the  ERRMAX  array  have  been  found  suit¬ 
able  for  most  cases.  See  the  mathematical  diseussion  of  sub¬ 
routine  TRAJGX,  See.  IV,  for  a  description  of  the  eonvergenee 
errors. 
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III.  MAIN  PROGRAM  (VERSION  II) 


A  second  main  program  is  available  for  use  in  estimating  trajectory  parameters  from  real 
radar  measurement  data.  The  measurement  data  are  read  in  on  cards  but  it  is  a  simple  matter 
to  change  the  input  medium  and/or  format.  No  error  analysis  or  simulated  trajectory  genera¬ 
tion  is  done.  Four  input  cards,  followed  by  the  measurement  data  cards,  are  required  (see 
Table  III). 


TABLE  III 

INPUT  DATA  CARDS  TO  MAIN  PROGRAM  (VERSION  II) 

Card 

Format 

Argument  and/or  Description 

1 

18A4 

Title  card,  with  72  alpha-numerical  characters 
available. 

2 

4F10.3,  F5.3, 

315 

TZERO,  TINT,  TINCR,  DLAT,  PRNT,  NDATA, 

KLAMP,  MDATA  (see  text  below). 

3 

7F10.3 

(SIGMA  (J),  J  =  1,6),  COORD 

4 

7F10.3 

(ERRMAX(J),  J  =  1,  7)  convergence  errors 

5 

D15.2,  4D1  5.6 

Data  cards  for  each  measurement,  giving  time,  range, 
azimuth,  elevation,  range  rate 

The  title  card  and  the  arguments  TINCR,  DLAT,  PRNT,  NDATA,  KLAMP,  SIGMA,  and 
ERRMAX  are  exactly  as  described  previously  for  Version  I.  The  arguments  MDATA  and  COORD 
are  also  as  described  previously  except  that  MDATA  is  limited  to  positive  values  while  COORD 
must  equal  ±2. 

The  arguments  TZERO  and  TINT  specify  the  starting  time  of  the  desired  data  and  the  mini¬ 
mum  desired  data  interval,  respectively.  All  data  prior  to  time  TZERO  are  ignored,  as  well 
as  succeeding  data  points  which  are  less  than  TINT  seconds  later  than  the  previously  processed 
point. 

It  should  be  noted  that  an  initial  state  vector  to  start  the  program  is  obtained  from  the  meas¬ 
urements  at  time  TZERO  or  later.  Any  required  time  derivatives  not  contained  in  the  measure¬ 
ment  are  calculated  by  taking  differences  of  measurements  TINT  seconds,  or  more,  apart. 

A  block  diagram  of  this  program  is  given  in  Fig.  2. 

IV.  SUBROUTINE  TRAJGX 

A.  Description 

Subroutine  TRAJGX  is  a  double-precision  FORTRAN  subroutine  which  integrates  the  dif¬ 
ferential  equations  of  motion  in  radar-centered  rectangular  coordinates  over  any  specified  time 
interval,  using  a  predictor-corrector  method.  It  assumes  a  spherical  rotating  earth  with  a 
rigid  atmosphere.  A  choice  of  English  units  (feet,  pounds,  seconds),  or  metric  units  (meters, 
kilograms,  seconds),  is  available  by  specifying  the  parameter  COORD,  in  COMMON  storage. 
Atmospheric  data  are  obtained  by  calling  subroutine  DENS  via  the  entry  point  ATM,  described 
in  Sec.  VI.  Subroutine  TRAJGX  may  be  used  to  generate  a  ballistic  trajectory  or  to  predict  the 
position  and  velocity  of  a  re-entry  vehicle  at  any  given  time,  given  the  position,  velocity,  and 
ballistic  coefficient  at  some  other  time. 
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Fig.  2.  Flow  chart  of  TRAP  (version  II)  main  program. 
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Fig.  2.  Continued. 
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The  subroutine  is  called  by  the  following  statement: 


CALL  TRAJGX  (XI,  Yl,  Zl,  XDOT1,  YDOT1,  ZDOT1,  BETA1,  TAU, 
TSTEP,  ERRMAX,  X2,  Y2,  Z2,  XDOT2,  YDOT2,  ZDOT2,  BETA 2,  PllIMAT) 

The  input  arguments  are- 


initial  position  in  radar-centered 
rectangular  coordinates 


ZDOT1  =  ZlJ 

BETA1  =  initial  ballistic  coefficient 
TAU  =  integration  time  interval  (sec) 

TSTEP  =  integration  step  size  (sec) 

ERRMAX  =  convergence  errors  (see  description  below) 


The  output  arguments  are 


X2i 

Y2> position  at  time  TAU  seconds  after  initial  time 

zzj 

XDOT2  =  X2l 

vnnT?  -  v?  verity  components  at  time  TAU  seconds 
(after  initial  time 
ZDOT2  -  Z2 J 

BETA2  =  ballistic  coefficient  at  time  TAU  seconds  after  initial  time. 


(At  present,  TRAJGX  assumes  that  the  ballistic  coefficient 
is  constant  and  sets  BUTA2  =  BETA1.  Provision  can  be 
made,  however,  for  a  varying  ballistic  coefficient). 


PHIMAT  =  transition  matrix,  whose  elements  are  defined  by 


0 .  .  =  8x.(t  +  TALJ)/8x.(t  ) 

1J  l'  O  "  j  o 


where  is  the  ith  component  of  a  state  vector  x,  whose  components  are 


x,  (t  )  -  XI 
1  o 


x  .  (t  )  -  XDOT1 
4V  o 


x5(tQ)  =  YDOT1 


x6(tQ)  =  ZDOT1 


X  —  (t  )  =  1/BETA1 
1  o 


Similarly 
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xd(t  +  TAU)  =  X2 


x?(t  +  TAU)  =  1.0/BETA2 

The  following  statements  are  also  required  in  the  calling  program: 

COMMON/FCOM/COORD,  DLAT,  PRNT/lCOM/KLAMP,  MDATA,  NN 
DIMENSION  ERRMAX(7) 

where 


DLAT  =  latitude,  in  degrees,  of  radar  (reference)  site 

COORD  >  0.  indicates  English  system  units  of  feet,  pounds,  seconds 

COORD  <  0.  indicates  metric  system  units  of  meters,  kilograms, 
seconds 


NN  indicates  the  data  point  under  current  consideration  (NN  =  1 
for  the  first  data  point  and  indicates  that  TRAJGX  is  being 
called  for  the  first  time.  This  is  the  only  case  of  interest 
to  TRAJGX) 

ERRMAX  is  an  array  of  convergence  errors  associated  with  the  seven 
components  of  the  state  vector  x-  In  general,  the  smaller 
the  specified  convergence  errors,  the  more  accurate  the  in¬ 
tegration,  with  a  resulting  increase  in  computation  time. 

The  significance  of  the  convergence  errors  is  discussed  in 
Sec.  IV.  B.  Values  of  0.1  seem  to  be  suitable  for  all  seven 
convergence  errors. 


The  following  values  have  been  used  for  some  fundamental  parameters. 


Rj£  =  2.09257  38  X  10^  ft  (radius  of  the  earth) 

Gm  =  1.40763488  x  10^ft3/sec2  (Newton’s  gravitational 
constant  x  mass  of  earth) 


a; 


7.27  X  10 


radians/ sec 


(earth's  angular  rotation  rate) 


1  meter  =  3.2808333  ft 


7 r  =  3.14159265 


B.  Mathematical  Discussion 

The  program  assumes  the  following  basic  equations  of  motion  in  radar- centered  rectan¬ 
gular  coordinates:* 

x  =  Pax  +  Bx  +  Cy  +  Ez 
y  =  —  Cx  +  Gy  +  Pay  +  Jz  +  K 
z  =  —  Ex  +  Jy  +  Nz  +  Pa  z  +  Q 

where 


a  =  1  / /5  =  reciprocal  of  ballistic  coefficient  =  drag  parameter 


P  =  -pVD/ 2 


p  =  atmospheric  density 

T7  ...  .  •  2  ,  .2  ,  -2.1/2 

V^  =  drag  velocity  =  (x  +  y  +  z  ) 
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B  =  oj2  —  W  W  = 

C  =  2oj  sin  (DLAT) 

E  =  —  2a>  cos  (DLAT ) 

c2 

G=  V  “  W 

T*_  (C)(E) 

J - ? — 

K  =  (J)(Re) 

N=  \  -  W 
Q  =  (N)(re) 

The  position  and  motion  of  the  vehicle  are  described  in  terms  of  the  state  vector 


Gm/R3  (where  R  =  [x2  +  y2 
+  (z  +  Re)2]1/2) 


The  equations  of  motion  are  numerically  integrated  using  a  predictor-corrector  method,  which 

makes  use  of  the  state  vector  x,  the  vector  b  =  f(x)  =  8x/dt,  and  the  matrix  A  =  9f(x)/8x.  A 

2 

block  diagram  of  the  integration  scheme,  proposed  by  M.  Gruber,  is  shown  in  Fig.  3.  The  vec¬ 
tor  b  is  written  more  explicitly  below,  while  Table  IV  gives  the  matrix  A. 


b  = 


x 

y 

z 

X 

y 


z 

a 


Figure  3  shows  that  the  vector  x(tk)  at  time  t^,  is  integrated  over  one  step  size  to  time 
*k+l  a  Predicted  state  vector  which  is  based  on  knowledge  of  the  vectors  x(t^)  and 

x(tk  d).  A  corrected  vector  x(tk+^)  is  then  calculated,  using  the  newly  obtained  prediction 
-(tk+1 )  anc*  -^k^'  ^  convergence  errors,  defined  as  |g(t^+^)-“  x(tk+^)|,  are  less  than  the 

values  specified  in  the  ERRMAX  array  for  all  seven  components  of  x,  x(t^+1)  *s  accePted  as 
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Fig.  3.  Flow  chart  of  TRAJGX  calculations. 
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the  value  of  x  at  time  t^+1.  If  not,  is  set  equal  to  x(t^+1)  and  another  value  of  x(tk+1)  is 

calculated  based  on  x(t^)  and  the  corrected  vector  New  convergence  errors  are  calcu¬ 

lated  and  the  comparison  test  and  subsequent  procedure  are  repeated  until  sufficiently  small 
convergence  errors  are  obtained.  The  above  procedure  is  repeated  for  each  integration  step 
until  the  desired  interval  has  been  covered.  Figure  4  is  a  flow  chart  of  the  subroutine. 

V.  SUBROUTINE  ESTMAT 

A.  Description 

Subroutine  ESTMAT  estimates  the  true  value  of  a  state  vector  by  combining  a  predicted 
value,  given  in  radar-centered  rectangular  coordinates,  with  a  noisy  measurement  given  in 
radar-centered  polar  coordinates.  It  is  essentially  a  recursive  Kalman-filter  scheme.  Double¬ 
precision  calculations  are  used  throughout.  In  making  the  estimate,  the  prediction  and  the 
measurement  are  weighted  in  proportion  to  the  inverse  of  their  respective  mean-square  error 
covariance  matrices.  ESTMAT  may  also  be  used  to  perform  trajectory  error  analyses,  in 
which  the  effects  of  hypothetical  errors  in  the  initial  state  and  subsequent  measurements  are 
ealeulated.  Suitable  variances  are  inserted  in  the  appropriate  covariance  matrices  which  are 
evaluated  and  propagated  along  the  nominal  trajectory. 

The  subroutine  calculates  the  mean-square  error  covariance  matrix  of  the  estimate,  and 
prints  it  out  if  desired.  This  matrix  is  saved  by  the  subroutine  to  be  used,  along  with  the  tran¬ 
sition  matrix,  in  computing  the  covariance  matrix  of  the  next  prediction.  The  transition  matrix, 
defined  by  the  equation 

*k,k-i  =  8S(‘k>/®S<‘k-l> 

is  required  as  an  input  argument  to  ESTMAT.  It  is  computed  by  subroutine  TRAJGX  in  the 
course  of  calculating  the  predicted  value  of  x(t^)-  If  TRAJGX  is  not  used,  a  suitable  transition 
matrix  must  be  calculated  elsewhere. 

Subroutines  DMTMUL,  MINV,  RCOVTX,  XCOVTR,  and  XYTOR  are  also  required  by 
ESTMAT. 

One  problem  that  arises  in  linear  recursive  tracking  is  that  at  a  sufficiently  high  data  rate, 
the  computed  covariance  matrix  of  the  estimate  will  tend  towards  zero.  This  has  the  effect  of 
giving  progressively  less  weight  to  measurements.  As  a  result  of  errors  in  our  model  of  the 
state  equation,  or  in  our  covariance  matrix  calculations,  or  in  the  numerical  integration  rou¬ 
tine  (especially  during  re-entry),  it  is  often  advisable  to  weight  the  measurements  higher  than 
the  algorithm  normally  would. 

An  arbitrarily  chosen  deviee  (others  are  possible)  that  has  been  used  to  remedy  the  situa¬ 
tion  is  to  prevent  the  determinant  of  the  covarianee  matrix  from  deereasing  below  a  given  ref¬ 
erence  value.  This  reference  is  generally  set  equal  to  the  determinant  of  the  covariance  matrix 
after  a  specified  number  of  data  points  have  been  processed.  For  all  subsequent  estimates, 
the  determinant  of  the  covariance  matrix  is  calculated  and  the  entire  covariance  matrix  is  scaled 
up  so  that  its  determinant  is  equal  to  the  reference.  The  determinant,  and  also  the  covariance 
matrix,  are  said  to  be  "clamped,”  and  prevented  from  decreasing  to  zero.  A  parameter  KLAMP, 
referred  to  as  the  clamping  time,  or  memory,  of  the  algorithm  specifies  the  number  of  data 
points  to  be  processed  before  calculating  the  reference  determinant. 
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Fig.  4.  Flow  chart  of  subroutine  TRAJGX. 
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Fig.  4.  Continued. 
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Subroutine  ESTMAT  is  ealled  by  the  following  statement- 

CALL  ESTMAT  (STATEM;  STATEP,  PHIMTX,  ESTATE,  DEVX,  DEVR) 

The  input  arguments  are: 

STATEM  Measurement  veetor  in  radar- eentered  polar  coordinates 
STATE M(1 )  =  range  measurement 
STATEM(2)  =  azimuth  measurement  (in  radians) 

STATEM(3)  =  elevation  measurement  (in  radians) 

STATEM(4)  =  range  rate  (Doppler)  measurement  (if  used) 

STATEM(5)  =  azimuth  rate  measurement  (if  used) 

STATEM(6)  =  elevation  rate  measurement  (if  used) 

STATEM(7)  :  generally  not  used 

STATEP  Predicted  state  veetor  in  radar-eentered  rectangular 
coordinates 

PHIMTX  Transition  matrix  (Note:  this  matrix  is  destroyed  by  ESTMAT) 

The  output  arguments  are: 

ESTATE  Estimated  state  vector  in  radar-centered  rectangular 
coordinates 

DEVX  Seven-eomponent  veetor  containing  expected  rms  errors 
of  estimated  state  veetor  in  rectangular  coordinates 

DEVR  Seven-eomponent  vector  containing  expected  rms  errors 
of  estimated  state  vector  in  radar  polar  coordinates 

The  following  statements  are  also  required  in  the  ealling  program: 

COMMON /ACOM/COVAR (7),  SIGMA(7 )/FCOM/COORD,  DIJVT,  PRNT/ 

ICOM/KLAMP,  MDATA,  NN' 

DIMENSION  STATEM(7),  STATEP(7),  ESTATE(7),  PHIMTX(7,  7), 

DEVX (7 ),  DEVR (7 ) 

The  quantity  MDATA  is  an  integer  specifying  the  mode  of  operation  of  ESTMAT,  and  may 
have  values  from  —1  to  +7,  but  not  zero.  (If  MDATA  =  0,  noiseless  trajectory  data  will  be 
generated  and  subroutine  ESTMAT  will  not  be  called.)  The  magnitude  of  MDATA  indicates  the 
number  of  measurement  veetor  components  used,  while  the  sign  indicates  the  type  of  job  to  be 
done  —  positive  for  estimation,  negative  for  error  analysis.  Some  eommon  values  are  given 
below: 


MDATA  =  4  state-vector  estimation,  using  range,  azimuth, 

elevation,  and  range  rate  (Doppler)  measurements 

MDATA  =  3  state-vector  estimation,  using  range,  azimuth, 
and  elevation  measurements 


MDATA  =  1  state-vector  estimation,  using  range  measurements  only 

MDATA  =—  4  error  analysis,  assuming  range,  azimuth,  elevation, 
and  range  rate  (Doppler)  measurements 

MDATA  =  —  3  error  analysis,  assuming  range,  azimuth,  and 
elevation  measurements 


MDATA  =  —  1  error  analysis,  assuming  range  measurement  only. 


The  SIGMA  array  contains  the  rms  errors  associated  with  the  measurement  veetor. 
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SIGMA(l)  =  rms  range  measurement  error 
SIGMA(Z)  =  rms  azimuth  measurement  error  (in  radians) 
SIGMA(3)  =  rms  elevation  measurement  error  {in  radians) 
SIGMA(4)  =  rms  range  rate  (Doppler)  measurement  (if  used) 
SIGMA(5)  =  rms  azimuth  rate  measurement  error  (if  used) 
SIGMA(6)  =  rms  elevation  rate  measurement  error  (if  used) 
SIGMA(7)  ;  not  used 


The  COVAR  array  contains  the  rms  errors  associated  with  the  initial  estimate  of  the  state 
vector.  (At  the  start  of  a  run,  an  initial  estimate  of  the  state  vector,  along  with  rms  errors 
associated  with  it,  must  be  given.)  The  elements  of  the  COVAR  array  may  be  in  radar-centered 
rectangular  or  polar  coordinates  and  are  listed  below. 


COVAR  (1 )  = 
COVAR(Z)  = 
COVAR (3)  - 
C  O  V  A  R  ( 4 )  - 
COVAR(5)  = 
COVAR  (6)  = 
COVAR(7)  - 


rms  error  in  initial  estimate  of  x 
rms  error  in  initial  estimate  of  y 
rms  error  in  initial  estimate  of  z 
rms  error  in  initial  estimate  of  x 
rms  error  in  initial  estimate  of  y 
rms  error  in  initial  estimate  of  z 
rms  error  in  initial  estimate  of  a 


(or  range) 

(or  azimuth) 

(or  elevation) 

(or  range  rate) 

(or  azimuth  rate) 

(or  elevation  rate) 

=  drag  parameter  =  1  / >3 


The  specification  of  the  coordinate  system  of  COVAR  is  indicated  by  the  value  of  the  pa¬ 
rameter  COORI). 


COORD  =  ±1.0  COVAR  in  rectangular  coordinates 
COORD  -  ±Z.O  COVAR  in  polar  coordinates 


A  selection  of  printed  output  is  available  by  specification  of  the  parameter  PRNT. 


PRNT  <0.  No  printed  output  from  ESTMAT 

PRNT  =  0.  The  covariance  matrix  of  the  estimated  state  vector 
is  printed  for  each  point  processed.  It  is  given  in 
rectangular  coordinates  along  with  a  ’’hybrid"  matrix 
in  polar  coordinates.  In  the  hybrid  matrix,  the  diag¬ 
onal  terms  are  rms,  rather  than  mean-square,  errors 
while  off-diagonal  terms  are  correlation  coefficients 
of  the  mean-square  errors.  Mathematically, 


e. . 

li 


c. . 
D 


(diagonal  terms) 


(off-diagonal  terms) 


where 

cr^  =  mean  square  covariance  in  polar  coordinates 
e  =  hybrid  matrix  element 


PRNT  >  0.  Covariance  matrices  for  predicted  as  well  as  estimated 
state  vectors  are  printed  for  each  data  point  in  the  same 
formats  described  above.  In  addition,  the  partial  deriv¬ 
ative  matrix  II,  the  weighting  matrix  W  (see  Sec.  V.  13), 
and  the  estimated  state  vector  in  rectangular  coordinates 
are  printed. 


19 


Other  quantities  in  COMMON  storage  are: 

NN  =  number  of  the  data  point  currently  being  processed. 

KLAMP  =  clamping,  or  memory,  time  of  process  note:  KLAMP  -  0 
is  used  to  indicate  that  no  clamping  is  desired.  The  same 
effect  may  be  obtained  by  making  KLAMP  greater  than  the 
total  number  of  points  to  be  processed. 

DLAT  =  latitude  of  radar  (reference)  site,  in  degrees  (not  used  by 
ESTMAT) 

B.  Mathematical  Discussion 


As  stated  in  part  (A)  of  this  section,  ESTMAT  estimates  the  true  value  of  a  state  vector  by 
combining  a  predicted  value  of  the  state  vector  with  a  noisy  measurement  vector.  The  mathe¬ 
matical  formula  is  given  below: 


where 


A 

*k  = 


*k,  k-1  +  W[£k-h(xkj  k.1)l 

(1) 

-1  T  -1  -1  T  -1 

(sk;k-i  +  H  R  H)  H  R 

(2a) 

T  -1 

SkHTR 

(2b) 

A 

X, 


estimate  of  state  vector  x  at  time  t. 


-k,  k-1 


=  predicted  state  vector  x  at  time  t^,  based  on 

x,  , ,  the  estimate  of  x  at  time  t,  . 
k-1  —  k-1 


r k  =  measurement  vector  at  time 

=  mean-square  error  covariance  matrix  of  x^ 
where  (S^..  =  E{[($k).  -  (x*)^  [(^  -  (x*).J  1  } 
and  x*  =  nominal  (noiseless)  value  of  x. 


-k,  k-1 


mean-square  error  covariance  matrix  of  x 


k,  k-1 


H  =  mean-square  error  covariance  matrix  of  measure¬ 
ment  vector  r. 

H  =  partial  derivative  matrix  dr/dx, 

where  r  =  vector  in  measurement  coordinates 
x  =  vector  in  estimation  coordinates 
r  =  h(x) 

If  r  has  m  components  and  x  has  n  components 
H  will  have  m  rows  and  n  columns.  If  r  and  x 
are  in  the  same  coordinate  system,  H..  l<5(i  —  j) 


Equation  (1)  is  approximately  equivalent  to 

xk  k-1  +  HTR_1rkl 


(3) 


Equation  (3)  shows  that  the  prediction  x^  ^  ^  and  the  measurement  r^  are  each  weighted  by 
the  inverse  of  their  respective  covariance  function.  The  matrix  is  related  to  the  other  ma¬ 
trices  by  the  relation 
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(4) 


-k  =  (-k!k-l  +  — T-  )_1 


and  S^  ^  ^  is  related  to  the  covariance  matrix  of  the  estimate  at  the  previous  point  by 
— k,  k- 1  “  -  k,  k-1— k-1  —  k,  k-1 

where 


(5) 


-k,  k-i =  9-k,  k-i  9-k-i 


(6) 


The  matrix  4>  is  called  the  transition  matrix  and  is  calculated  in  subroutine  TRAJGX. 
Figure  5  shows  a  flow  chart  of  version  I  of  ESTMAT,  based  on  Eqs.  (i  )  and  (Z) 
Emphasis  is  on  the  inverse  covariance  matrix  S  *,  which  is  continually  updated  by  the 
equation 


-1  -IT  -1  -1 

S,  .  ,  =  4>,  ,  ,S,  .  4 

-k,  k-1  —  k,  k-1— k-1— k,  k-1 


(7) 


Matrix  inversion  is  performed  to  obtain  ^  for  use  in  Eq.  (7)  and  to  calculate  the  weight¬ 

ing  matrix  W  of  Eq.  (Z).  Since  x  is  a  seven-component  vector,  both  of  these  inversions  involve 
7X7  matrices. 

V.O.  Mowery^  has  presented  some  alternate  equations,  mathematically  identical  with 
Eqs.  (Z)  and  (4),  which  involve  inversion  of  lesser  order  matrices.  Replacing  Eqs.  (Z)  and  (4), 
we  have 


—  =  -k,  k-i— T(— k  +  — — k,  k-i— T)_1  <8> 

-k  =  d-WHJSk.k.!  (9) 


where 


1^  =  identity  matrix 

The  matrix  to  be  inverted  is  now  n  x  n  where  n,  the  dimension  of  the  measurement  vector  r, 
is  practically  always  less  than  7.  Version  II  of  ESTMAT,  using  Mowery' s  equations,  is  shown 
in  Fig.  6. 

In  performing  error  analyses,  all  matrices  are  evaluated  along  the  nominal  trajectory,  with 
the  covariance  matrices  being  updated  by  Eqs.  (7)  and  (4),  or  (5)  and  (9)  depending  on  which  ver¬ 
sion  of  ESTMAT  was  used. 


VI.  SUBROUTINE  DENS 
A.  Description 

Subroutine  DENS  computes,  in  double  precision,  the  atmospheric  density  at  any  altitude. 
The  subroutine  may  be  entered  through  two  points,  DENS  and  ATM.  The  calling  statements  for 
each  entry  point  are  given  below  with  explanation. 

The  statement  CALL  DENS  must  be  used  to  enter  the  program  initially,  before  any  atmos¬ 
pheric  density  data  are  required.  It  sets  up  arrays  of  up  to  100  discrete  altitudes  in  km  and  kft, 

3  3 

and  arrays  of  their  corresponding  atmospheric  densities  in  kg/meter  and  lb/ft  ,  respectively. 

*  Atmospheric  density  data  are  from  the  U.  S.  Standard  Atmosphere  196Z  (United  States  Gov¬ 
ernment  Printing  Office,  Washington,  D.  C.,  196Z),  Tables  I  and  IV. 
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Fig.  5.  Flow  chart  of'  subroutine  ESTMAT. 
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Fig.  5.  Continued. 
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Fig.  6.  Flow  chart  of  subroutine  ESTMAT  (Mowery  method). 
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Fig.  6.  Continued. 
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(As  presently  written,  only  the  first  61  elements  of  each  array  are  filled  in,  up  to  an  altitude  of 
about  200km.  At  higher  altitudes  the  atmospheric  density  is  set  equal  to  zero.)  Control  is  then 
returned  to  the  main  program. 

The  statement  CALL  ATM(HKMFT,  RIIO)  is  used  when  atmospherie  density  data  are  re¬ 
quired.  The  arguments  are 

HKMFT  =  altitude  in  km  or  kft 

RHO  =  density  in  kg/m^  or  lb/ ft^ 

The  units  used  will  depend  on  the  value  of  the  parameter  COORI),  in  COMMON  storage. 
English  units  are  used  if  COORD  >  0,  metrie  units  if  COORD  <  0. 

The  following  COMMON  statement  is  also  required  in  the  calling  program: 
COMMON/FCOM/COORl),  dlat,  prnt 
The  other  quantities  in  COMMON  storage  are  not  used  by  this  subroutine. 


H.  Mathematical  Diseussion 


The  given  altitude  is  compared  with  those  stored  by  the  subroutine,  and  the  desired  density 
is  computed  as  follows: 


If 


then 


and 


where 


h.  <  H  <  h.,, 
j  j+i 


A  -  (II  -  h.)/(h.J_,  -  h.) 

J  J+l  J 


II  input  altitude,  in  kilometers  or  kilofeet 
hj  j^1  altitude  in  stored  array 

p  ~  atmospherie  density  in  stored  array, 
^  corresponding  to  fr 


p  atmospheric  density  at  altitude  H 

If  II  >  h  ,  where  h  =  maximum  altitude  stored  in  arrav,  then  p  0.  If  II  <  0,  tht  pro- 

j  max  j  max  ^  1 

gram  prints  out  EARTH  IMPACT. 


VII.  SUBROUTINE  XYTOR 

Subroutine  XYTOR  converts  position  and  velocity  given  in  radar-centered  rectangular  co¬ 
ordinates  into  radar-eentered  polar  coordinates.  The  following  calling  statement  is  required: 

CALL  XYTOR(X.  Y,  Z,  XDOT,  YI)OT,  ZDOT,  R,  A,  E.  RDOT,  ADOT.  EDOT) 
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The  input  arguments  are: 

X  =  position  component  in  easterly  direction 
Y  =  position  component  in  northerly  direction 
Z  =  position  component  in  vertical  direction 
XDOT  =  X  =  dX/dt 
YDOT  =  Y  =  dY/dt 
ZDOT  =  Z  =  dZ/dt 


The  output  arguments  are: 

R  =  range 

A  =  azimuth  (in  radians) 

E  =  elevation  (in  radians) 

RDOT  =  range  rate  =  R  -  dR/dt 
ADOT  =  azimuth  rate  (radians/sec)  =  A  =  dA/dt 
EDOT  =  elevation  rate  (radians/sec)  =  E  =  dE/dt 
The  following  equations  are  used  in  the  calculations: 


R  =  (X2  +  Y2  +  Z2)1  'Z 
A  =  tan-1  (X/Y) 

E  =  sin_1(Z/R) 

RDOT  =  (XX  +  YY  +  ZZ)/R 

ADOT  =  (YX  +  XY)/(X2  +  Y2) 

EDOT  =  (RZ  -  ZR)/[R(R2  -  Z2)1  2] 

VIII.  SUBROUTINE  RTOXY 


Subroutine  RTOXY  converts  position  and  velocity  given  in  radar-centered  polar  coordinates 
to  radar-centered  rectangular  coordinates.  The  following  calling  statement  is  required: 

CALL  RTOXY (R,  A,  E,  RDOT,  ADOT,  EDOT,  X,  Y,  Z,  XDOT,  YDOT,  ZDOT) 

The  input  arguments  are: 

R  =  range 

A  =  azimuth  (in  radians) 

E  =  elevation  (in  radians) 

RDOT  =  range  rate  =  R  -  dR/dt 

ADOT  =  azimuth  rate  (radians/sec)  =  A  =  dA/dt 

EDOT  =  elevation  rate  (radians/sec)  =  E  =  dE/dt 
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The  output  arguments  are: 


X  =  position  component  in  easterly  direction 

Y  =  position  component  in  northerly  direction 
Z  =  position  component  in  vertical  direction 

XDOT  =  X  =  dX/dt 
YDOT  =  Y  =  dY/dt 
ZPOT  =  Z  =  dZ/dt 
The  following  equations  are  used 
X  =  R  eos  E  sin  A 

Y  =  R  eos  E  eos  A 
Z  =  R  sinE 

XDOT  =  XR/R  -  ZE  sinA  +  YA 
YDOT  =  YR/R  -  ZE  eos  A  -  XA 
ZDOT  =  ZR/R  +  RE  eosE 

IX.  SUBROUTINES  XCOVTR  AND  RCOVTX 
A.  Description  of  XCOVTR 

Subroutine  XCOVTR  converts  a  7  x  7  mean-square  error  covariance  matrix  in  radar- 
centered  rectangular  coordinates  to  one  in  polar  coordinates.  It  also  calculates  a  partial  de¬ 
rivative  matrix  H  =  Or/ Ox  where  r  and  x  are  the  state  veetors  in  radar-eentered  polar  and 
rectangular  coordinates,  respectively. 

The  ealling  statement  is 

CALL  XCOVTR (XCOV,  XVCTR,  RCOV,  NCODE) 

The  input  arguments  are: 

XCOV  =  7X7  covariance  matrix  in  rectangular  coordinates 

XVCTR  -  seven-component  state  vector  used  in  evaluating 
matrices 

NCODE  =  operation  selector  code 

NCODE  >  0-  subroutine  calculates  eovariance  matrix 
in  polar  coordinates 

NCODE  <  0;  subroutine  calculates  partial  derivative 
matrix  H  =  Or/ Ox 

The  output  argument  is: 

RCOV  =  7x7  mean-square  error  covariance  in  radar-eentered 
polar  coordinates 

or 

partial  derivative  matrix  H  =  Or/ Ox 
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The  following  statement  is  also  required  in  the  calling  program: 

DIMENSION  XCOV(7,  7),  RCOV(7,  7),  XVCTR(7) 

Subroutines  XYTOR  and  DMTMUL  are  required  by  this  subroutine. 

B.  Description  of  RCOVTX 

Subroutine  RCOVTX  converts  a  7  x  7  mean-square  error  covariance  matrix  in  radar- 
centered  polar  coordinates  to  one  in  rectangular  coordinates.  The  calling  statement  is: 

CALL  RCOVTX  (COVR,  XSTATE,  COVX) 

The  input  arguments  are: 

COVR  =  7x7  mean-square  error  covariance  matrix 
in  polar  coordinates 

XSTATE  =  seven-component  state  vector  used  in  evaluating 
matrices 

The  output  argument  is: 

COVX  -  7  x  7  mean-square  error  covariance  matrix 
in  radar-centered  rectangular  coordinates 

The  following  statement  is  also  required  in  the  calling  program: 

DIMENSION  COVR (7,  7 ),  COVX(7,  7),  XSTATE(7) 

Subroutine  XYTOR  is  required  by  this  subroutine. 

C.  Mathematical  Discussion 

Consider  the  state  vectors  x  and  r  in  radar-centered  rectangular  and  polar  coordinates, 
respectively. 

x:  x^  =  x  easterly  component  r:  r^ 

x^  =  y  northerly  component  r^ 

x^  =  z  vertical  component  r^ 

x4  =  x  r4 

x5  =  y  r5 

x6  =  *  r6 

x^  =  a  =  i/p  =  reciprocal  of  r^ 

ballistic  coefficient 

The  covariance  matrices  are 

T 

=  E  [(x  —  x*)  (x  —  x*)  ]  in  rectangular  coordinates 
T 

£  =  E  [ (r  —  r* )  (r  —  r^ )  ]  in  polar  coordinates 

where 

E  indicates  average  or  expected  value 
x*  =  nominal  value  of  x 
r*  =  nominal  value  of  r 


=  R  range 
=  A  =  azimuth 
=  E  =  elevation 
=  R 
=  A 
=  E 

=  a  -  i/P 
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The  covariance  matrices  2^  and  2^  are  related  by  the  equations 
tT 


2  =H2  HJ 

-r  — x— 


2  =  B2  BJ 

—  x r— 


H  =  Or/ Ox 

H..  =  Or  ./Ox. 
ij  v  i 

B  =  Ox/ Or  =  H 


-1 


B..  =  Ox./O  r. 
ij  i  J 

where  the  subscripts  indicate  the  component  (or  element)  of  the  veetor  (or  matrix)  involved. 

The  first  six  components  of  the  x  and  r  vectors  are  related  by  the  equations  given  with 
subroutines  XYTOR  and  RTOXY,  while  x 7  -  r^.  The  various  elements  of  the  H  and  B  matrices 
are  given  below: 


Hn  =  1  =  x/R 


H 


OR 
12  -  9y 


y/R 


H13=  fz  =  Z/R 
H14  =  through  H17  =  0 


H  =  —  =  - X- — - 

n21  dx  2  2 

x  +  y 


II 


2  2 
x  +  y 


OA 

'  ay 

H  .  =  IT  =  0 

23  Oz 


II^4  through  H27  =  0 


H 


OE 
31  Ox 


H 


H 


OE 

32  Oy 


OE 

33  ’  Oz 


/  2  7  2 

*  x  +  y 


yz 

/  2  7  2 

vx  +  y 


R 


V  X2  +  y2 

R2 


II 


42  Oy 


OR  Ry  -  Ry 


R 


II 


OR  Rz  Rz 


4  3  Oz 


H..  =  H, , 
44  11 


H45  =  H12 


H46=H13 


H47  ~  ° 


R 


H 


II 


OA 

51  Ox 


(2xA  4 
x  +  y 


OA  x  —  2Ay 

52  =  Oy  ~  2  2 

x  +  y 


„  _  9A  _  0 

H53  Oz  ° 


H54  "  H21 


H55  "  H22 


n56  =  »23 


H57  =  0 


through  H^7  =  0 


H 


OR  Rx  —  Rx 


41  Ox 


R 


30 


H6l  ^=- 


8E 

dx 


xE 


(x2  +  y2) 


_  2R  „  _ 

R  11 31 


2  /  2  A  2 
v  x  +  y 


R 


_  9E  _  yE  2R  „  zy 

62  “  8y  '  ,  2  x  2"  R  32  I  7  = 

J  (x  +  y  )  _,2  /  2  ,  2 

v  J  '  R  V  x  +  y 

8E  zE  R  „ 

63-  3z-  r2  RH33 


H 

H 

H 

H 

H 

H 


64 

65 

66 
67 
71 

77 


=  H 


31 


=  H 


32 


=  H 


33 


=  0 


through  =  0 
=  da /da  =  1 


B  .  =  3x/8R  =  cos  E  sin  A 

1 1 

“  8x/ 8A  =  R  cosE  cos  A  =  y 
B  ^  =  8x/ 8E  =  —  R  sin  E  sin  A  =■  —  z  sin  A 
B14  through  B1?  -  0 
B^  -  8y/8R  =  cosE  cosA 
B^  =  9y/9A  =  —  R  cosE  sin  A  =  — x 
^23  =  =  —  R  sinE  cos  A  =  z  cos  A 

BZ4  through  B^7  =  0 
B^  =  8z/8R  =  sinE  =  z/R 
B32  =  8z/8A  =  0 
B33  =  8z/8E  =  R  cosE 
B34  through  B37  =  0 

B^j  =  8x/8R  =  A  cosE  cos  A  —  E  sinE  sin  A 

B^^  =  dx/  8A=  R  cosE  cos  A  —  ER  sin  E  cos  A  —  AR  cosE  sin  A 
=  R  cosE  cos  A  —  zE  cos  A  —  xA 
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B43 

:  —  R  sin  E  sin  A  —  ER  cos  E  sin  A  —  AR  sin  E  cos  A 

B44  ~ 

:  —  R  sin  E  sin  A  —  xE  —  zA  cos  A 

:  3x/ 3R  =  3x/3R  =  B^ 

B45 

!  3x/3A  =  3x/3A  =  B12 

B46  = 

3x/3E  =  3x/3E  =  B13 

• 

B47 

:  0 

B51 

:  by/ 3R  =  —  E  sin  E  cos  A  —  A  cos  E  sin  A 

B52 

-  3y/3A  =  —  R  cos  E  sin  A  +  ER  sin  E  sin  A  —  AR  cos  E  cos  A 

=  —  R  cos  E  sin  A  +  zE  sin  A  —  yA 

B53  = 

:  3y/3E  =  — R  sinE  cosA  —  ER  cosE  cosA  +  Ar  sinE  sinA 

B54 

:  —  R  sin  E  cos  A  —  yE  +  zA  sin  A 

:  3y/3R  =  3y/3R  = 

B55 

■  3y /9A  =  3y/3A  =  B22 

B56  = 

:  3y/3E  =  3y/3E  =  B23 

B57 

=  0 

B61 

:  3z/ 3R  =  E  cos  E 

B62  = 

=  3  z/ 3 A  =  0 

B63 

=  3z/ 3E!  =  R  cos  E  —  ER  sin  E 

=  R  cos  E  —  zE 

B64 

=  3z/3R  =  3z/3R  =  B31 

B65 

=  3z/3A  =  3z/3A  =  B32 

B66  : 

=  3z/3E  =  3z/3E  =  B33 

B67 

:  0 

B?1  through  =  0  » 

B??  =  da /da  =  1 

<* 

X.  SUBROUTINE  GAUSSN 
A.  Description 

Subroutine  GAUSSN  generates  random  numbers  with  a  Gaussian  distribution  of  zero  mean 
and  standard  deviation  SIGMA.  The  program  is  called  by 
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I 


CALL  GAUSSN(NS,  RN,  SIGMA) 

where 

NS  =  number  of  samples  desired 
RN  =  array  name  of  output 
SIGMA  =  desired  standard  deviation 

B.  Mathematical  Discussion 

The  subroutine  makes  use  of  the  library  subroutines  RAN  and  ALOG.  The  procedure  used 

4 

to  obtain  a  Gaussian  number,  RN,  is  as  follows: 

Let  y  =  -  In 

z  =-ln  V2 

where  V  and  V  are  random  numbers  obtained  from  the  RAN  subroutine. 

1  z  2  2 
The  quantity  (y  —  1)  is  compared  with  2z.  If  (y  —  1 )  >  2z,  two  new  random  numbers  are 

2 

selected  and  the  above  comparison  repeated.  If  (y  —  1 )  <  2z,  a  third  random  number  S  is  se¬ 

lected  to  determine  the  sign  of  the  desired  output  number.  If 

S  <  0.5,  RN  =  (y)  (SIGMA) 

S  >  0.5,  RN  =  —  (y)  (SIGMA) 

S  =  0.5,  another' number  S  is  chosen  and  the  above 
test  for  sign  is  repeated. 

XL  SUBROUTINE  DMTMUL 

Subroutine  DMTMUL  is  a  double-precision  matrix  multiplication  subroutine.  It  is  called  by 
CALL  DMTMUL(A,  B,  AB,  NRA,  NCA,  NCB). 

The  input  arguments  are: 

A1 

pi  input  matrices  to  be  multiplied 

NRA  =  number  of  rows  in  matrix  A 
NCA  =  number  of  columns  in  matrix  A 
NCB  =  number  of  columns  in  matrix  B 
The  output  argument  is 

AB  =  matrix  product'  (A )(B) 

The  matrices  A,  B,  and  AB  must  have  the  same  dimensions  in  both  the  calling  program 
and  the  subroutine.  When  used  with  the  TRAP  program  described  here.  A,  B,  and  AB  are  di¬ 
mensioned  7X7.  Smaller  matrices  may  also  be  multiplied  by  DMTMUL,  even  though  they  may 
not  fill  the  complete  array,  by  proper  specification  of  NRA,  NCA,  and  NCB. 

XU.  SUBROUTINE  M1NV 

Subroutine  MINV  is  a  matrix  inversion  subroutine  from  the  IBM  System/360  Scientific  Sub¬ 
routine  Package.^  It  uses  the  standard  Gauss-Jordan  method  and  is  available  in  both  single  and 
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double  precision.  (Our  applications  use  the  double-precision  version.)  The  subroutine  is  called 
by  the  statement: 

CALL  MINV (A,  N,  D,  L,  M) 

where 

A  =  input  matrix,  destroyed  in  computation 
and  replaced  by  resultant  inverse 

N  =  order  of  matrix 

D  =  resultant  determinant 

1=  work  vectors  of  length  N 

The  one-dimensional  array  variables  A,  L  and  M  must  be  suitably  dimensioned  in  the  call¬ 
ing  program. 

Z 

The  matrix  to  be  inverted  may  be  stored  in  a  one-dimensional  array  of  size  N  before  call¬ 
ing  MINV ^  A  series  of  FORTRAN  statements  to  do  this  are  given  below: 

DO  1  J  =  1,  N 

DO  1  K  =  1,  N 

L  =  N*  ( J  -  1 )  +  K 

1  A(L)  =  ARRAY(J,  K) 

where  ARRAY  is  the  two-dimensional  array  of  the  matrix  to  be  inverted. 

Subroutine  MINV  may  now  be  called  to  invert  the  matrix  stored  in  the  one-dimensional 
array  A.  After  matrix  inversion,  the  resulting  inverse,  stored  in  the  one-dimensional  array 
A,  may  be  placed  in  a  two-dimensional  array  by  the  following  statements: 

DO  2  L  =  1,  LMAX 
J  =  (L-  1  )/N  +  1 
K  =  L  -  N*  (J  -  1) 

2  ARRAYI(J,  K)  =  A(L) 

Z 

where  LMAX  =  N  and  ARRAYI  is  the  N  x  N  array  of  the  inverted  matrix. 
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APPENDIX  A 

SOME  RELATIONS  USED  IN  CALCULATION  OF  DERIVATIVES 
FOR  A  MATRIX  8f(x)/8x 


p  =  pQe  =  atmospheric  density 

h  =  [x2  +  y2  +  (z  +  Re)2)1//2  -  Re  =  altitude 
Rg  =  radius  of  earth 

=  (x2  +  y2  +  z2)1/2  =  drag  velocity 

22  2l/2 

R  =  [x  +  y  +  (z  +  R]p)  ]  -  distance  from  center  of  earth  to  object 

9P/3x  =  -  Py  ^ 

ap/9y  =  -  Py  ^ 

9p/az  =  -  Py (z  +  re)/r 
ap/9x  =  Px/V2 

ap/9y  =  Py/v2 

ap/3z  =  Pz/v  2 

B  =  u2-Gm/R5 
G  =  CO2  sin2(DLAT)  -  Gm/R3 
N  =  u>2  cos2(DLAT)  -  Gj^/R^ 
au/ax  =  aN/ax  =  3xGm/rb 
3B/ay  =  dG/dy  =  9N/3y  =  SyGj^/R3 
dB/dz  =  dg/dz  =  3N/3z  =  3(z  +  RR)  GM/RS 
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APPENDIX  B 
PROGRAM  LISTINGS 
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TABLE  B-I 

MAIN  PROGRAM  (VERSION  I) 


C 

C 

C 

C 

C 

C 


MAIN  PROGRAM 

IMPLICIT  REAL*8( A-H,0-Z) 

COMMON  /ACCM/CCVAR( 7) ,SIGMA( 7 ) /FCOM/COORD , DLAT , PRNT/ICOM/KLAMP , MDA 
1  TA  ,  NN 

Cl  MENS  ION  GNC1SEI 7) ,RGN( 54 ) , AZN ( 54 ) , ELN ( 54 ) ,RAPN t 54 )  fBETAH (54)  f 

1  RHAT ( 54 ) , AHAT ( 54 ) »  EHAT ( 54 ) »  RPHAT ( 54 ) ,APHAT(54) ,  E  PHAT ( 54 ) ,  S TA T 
2EM(7) ,STATEP( 7) f PHIMATt  7,7) ,FNOISE( 7) , ALP HA (54) , DAL  PH (54) 

Cl  MENS  I  ON  PHINV(7, 7 ) , PROD ( 7, 7 ) , ESTATE ( 7  )  , STATE  I ( 7 )  , X ( 54 )  ,  Y ( 54 )  , Z ( 5 
14) ,XP( 54) ,YP( 54) ,ZP( 54) ,DX( 54) ,DY( 54 ) , DZ ( 54 ) , DXP ( 54 )  , D YP ( 54 ) , DZP ( 5 
24) »  XHAT ( 54) ,YHAT( 54  )  ,  Z  H  A  T  (  54  )  ,  XP  HA  T  (  54 ) ,YPHAT(54) , ZPHAT (54 ) , ASPECT 
3 (54) ,SIGR(54) f S IGA (54) f SIGE(54) ,SIGRP( 54) , SI  GAP (54) , SIGEP(54) , SIGX 
4(54) , S I  GY ( 54 ) f S I GZ ( 54 ) , S IGXP ( 54 ) , S I  GYP ( 54 ) , S I G ZP ( 54  )  , S I G AL P ( 54  ) 
CIMENSIGN  GN(7)tLABEL(  1 8 ) , RA ( 54 ) , AZ ( 54  )  , E L ( 54 )  ,  A ZP ( 54 )  , ELP ( 5 

14) f  HE IGHT ( 54 ) , T IME ( 54 ) , RAP ( 54) , BATA( 54 ) « ERRMAX ( 7)  , STDEVR(7)  ,  STDE V 

2  X  (  7  ) 

CIMENSIGN  DR( 54)  ,DE( 54 ) , CA ( 54 ) , DRP ( 54 ) , DAP (54)  , DEP(54)  ,DB(54) 

CALL  OLNS  TC  SET  UP  ATMOSPHERIC  DENSITY  TABLES 
CALL  DENS 

RE  AC  INPUT  DATA  CARDS 

1  REAC(5, 2, END= 9000) (LABEL! I), I =1,18) 

2  FORMAT  (  18A4  ) 

READ  (5, 3)  (  ST  AT  EH  J  )  ,  J=l,7) 

3  FORMAT ( 7F 10.3 ) 

REAC(5f4)TZERC, CELT, TINT, TI NCR, DLAT, PRNT,NDATA»KLAMP,MDATA 

4  FORMAT (5F10*3,F5.3,3I5) 

RE  AC ( 5 , 3 ) ( S I GMA ( J ) , J= 1,6)  , COORD 
R  E AD ( 5 , 3 ) (CCVAR(J),J=1,7) 

READ (5, 3) ( FNC I SE ( J ) ,J=1,7) 

READ (5, 3 ) ( ERRMAX ( JJ) , JJ= I ,7) 

C 

C  L ABEL=  IDENTIFYING  COMMENTS  TO  APPEAR  WITH  DATA  PRINT-CUT 

C  T  ZE  RC=  INITIAL  TIME 

C  T  INT=  INTERVAL  BETWEEN  DESIRED  DATA  POINTS  (SEC.) 

C  T I NCR= I NTEGK AT  ICN  STEP  SIZE  (SEC.) 

C  CL A  T=  LATITUDE  OF  RADAR  SITE  (IN  DEGREES) 

C  N D A T A=  NUMB E R  CF  DESIRED  DATA  POINTS 

C  PRNT=PRI NT -OUT  SELECTUR 

C  PRNT  =  - 1  •  MINIMUM  PRINT-DUT,  TABULATED  SUMMARY  GNLY 

C  PRN T=  0.  PRINT-OUT  OF  COVARIANCE  MATRICES  ♦  TABULATED  SUMMARY 

C  PRNT  =  I.  FULL  PRINT-OUT 

C  KLAMP= MEMORY  DF  ALGOR  I THM , EXPRESSED  AS  NUMBER  DF  DATA  POINTS 

C  MCAT  A  =  MCCE  SELECTOR 

C  MCATA  GREATER  THAN  C.  TRAJECTORY  PARAMETER  ESTIMATION 

C  MCATA  =  C.  NOISELESS  TRAJECTORY  GENERATION  CVNLY 

C  MCATA  LESS  THAN  -  =  0.  ERROR  ANALYSIS 

C  CCCRC  =  COORDINATE  AND  UNIT  SELECTOR 

C  CLGRC= -2 •  POLAR  COORDINATES,  METRIC  UNITS 

C  CCCRC=- 1 •  RECTANGULAR  COORDINATES,  METRIC  UNITS 

C  CCCRC=+ 1 •  RECTANGULAR  COORDINATES,  ENGLISH  UNITS 

C  CCQRD=+2 •  PCLAR  COORDINATES,  ENGLISH  UNITS 

C  SIGMA  =  RMS  NOISE  COMPONENTS  OF  MEASUREMENT  VECTOR 

C  CCVAR  =  RMS  NOISE  COMPONENTS  DF  INITIAL  STATE  VECTOR 
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TABLE  B-I  (Continued) 


C  FNCISE  =  NCISE  SAMPLES  ASSOCIATED  WITH  INITIAL  STATE  VECTOR 

C  ERRMAX  =  TRAJECTORY  INTEGRATION  CONVERGENCE  ERRORS  UN  REC- 

C  TANGULAR  COORDINATES ) 

C 

9  I M  AX=  5  0 

CCCRCM=DABS( CCCRC ) 

RTCEG=180./3.  14  159265 
C 

C  SET  UP  INITIAL  STATE  IN  BOTH  RADAR  POLAR  AND  XYZ  COORDINATES 

C 

I E ( CCCRDM- 1 • ) 400 ,400 ,300 
300  R  A (  1J-STATEI (  1 ) 

AZ  (  1  ESTATE  I  (  2  ) 

EL ( 1 )  =  ST  AT  E I ( 3) 

RAP(  1  )  =  S  T  A  T  E  I  (4) 

AZP (  1  )  =  STATE  I  ( 5  ) 

elp ( i  instate  i  ( 6 ) 

CALL  RTCXY ( RA(  1  ), AZ ( 1 ) ,EL (  I  ),RAP(  I) , AZP( I) ,ELP( 1) ,X( 1 )  ,Y( I ) ,Z(1 ) , X 
1P(  1  )  ,YP(  1  )  ,ZP(  1  )  ) 

GC  TO  401 

400  X (  1  ) =  STATE!  (  1  ) 

Y (  1 )=  STATE  I ( 2  ) 

Z (  1  )=  STATE  I  ( 3  ) 

XP(  I)'-STATEI(4) 

YP (  1  )  =  ST ATE  1(5) 

ZP (  1  )  =  ST  AT  E  I  ( 6  ) 

CALL  XYTCR(X(l),Y(l),Z(l),XP(i),YP(l),ZP(l),RA(l),AZ(l),EL(l),RAP( 
1  1 )  , AZP<  1  )  , CLP(  1  )  ) 

401  BETA=1./STATEI ( 7) 

BATA(  1  )  =  B  E  T  A 
EETA1  =  BATA(  1  ) 

C 

C  CALCULATE  ASPECT  ANGLE 

C 

A  S  P= ( X (  1  )*XP(1  )4Y(1  )*YP(1  )+Z(l  )*ZP(  1  )  ) / ( R  A  (  l  )*DSCRT( 

1XP(1  )**2  +  YP(l  )**2+ZP(  1  )**2)) 

ASPECT (  1  )=  180.-RTCEG*DAKC0S( ASP ) 

TIME(1)=TZERC 

TLAST-TZERC 

C 

C  PRINT  PLACER  PAGE  OF  OUTPUT  LISTING 

C 

WRITE  (6,  10)  (LABEL! I ) , 1  =  1 , 16) 

10  FORMAT ( •  1* ,2 OX, ’OUTPUT  L  1ST  ING* /  18A4// ) 

W  R  I  T  E  (  6  ,  1  1  ) 

11  FORMAT  ( /5X  ,  UNIT  I  AL  CONDITIONS'//) 

IF (CCCRO  )  14,  16, 16 

14  RE  =  2. 092573607/3.2608333 
W  R  I  T  E  (  6  ,  1  5  ) 

15  FCRMA1  (/5X, 'METRIC  UN  I  T  S  (  Ml  E  T  ER  S  ,  K  I  L  OG  R  AM  S  ,  R  AD  I  AN  S  ,  SEC  CND  S  )  USED  TH 
1RCUGH0UT  PROGRAM'//) 

C-C  TC  16 

16  RE=2.0925738C7 
WR  ITE (6,  17  ) 

17  FCRMAT(/5X, 'ENGLISH  UNITS(FEET, POUNDS, RADIANS, SECONDS)  USED  THKOUG 
1FCUT  PROGRAM'//) 

18  DC  19  J=  1 , 7 

19  STATE  (  J  )  =  0  • 

X  1  =‘X  (  1  ) 

Y  1  =  Y  (  1  ) 
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Z1  =  Z(  1) 

XCCT1=XP( 1 ) 

YDCT 1  =  Y  P ( 1 ) 

ZCCT  1  =  ZP (  I) 

hElGHT(l)=CSCRT(Xl**2+Yl**2+( Z  i  +  RE )**2 )-RE 
I  NDEX  =  1 

WR IT E (6,50  )  TIME (  INDEX ) , R A (  INDEX) , AZ( INDEX) , EL ( INDEX ) , RAP ( INDEX) ,AZ 
1  P (  INDEX), ELP(  INDEX), X  I , Y  1  , L  1 , XOOT  1  , YDO T  I  , ZDCT I , HEIGHT ( I NDE  X )  , BET  A 1 
WRITE (6,500 )ASPECT( I) 

500  FORMAT (5X, ' ASPhCT  ANGLE  =',F10.2,'  DEGREES*//) 

WRI TE (6, 1 10 ) CL AT, N CAT A, T I  NT, T I  NCR 

110  F  CRM AT (  / 5X  »  '  R AC  AR  L AT  I T UDE • , 3X , F 1 0 . 5 , 2X , • DEGREE S • /5 X , I  6 , 5 X , 1 

1CATA  POINTS  SPACE D 1 , 3X ,  F10.6  ,2X,'SEC.  APART,  INTEGRATION  STEP', 2 
2X,F10.6,2X, • SEC.*//) 

IF(MDATA)70,74,72 

70  WRITE(6,71) 

71  FCRMAT (/5X, 'ERROR  ANALYSIS'//) 

198  CC  201  J= 1 , 7 

201  FNCISL(J)=0. 

GC  TC  75 

74  W  R I TE ( 6 , 80 ) 

80  FCRMAT (/5X,  'TRAJFCTCRY  GENERATION'//) 

TAU  =  T I  NT 

GC  TC  81 

72  WRITE(6,73) 

73  FCRMAT (/5X, 'TRAJECTORY  PARAMETER  ESTIMATION  FROM  SI MLLATED  NOISY  D 
1  AT  A • / /  ) 

75  I  F  (  KLAMP) 760,760,  751 

751  WR I T  C ( 6  , 752)  KLAMP 

752  FCRMAT  ( /5X  ,' MEMORY  TIME  =',I6,'  DATA  POINTS'//) 

T  STEP=T I  NCR 

760  W  R  I  T  E  (  6  ,  76)  CELT 

76  FCRMAT  (/5X, ' T  IME  OF  INITIAL  MEASUREMENT  WITH  RESPECT  TC  TIME  OF  IN 

1ITIAL  ESTIMATE  *• ,F1C.3,3X, 'SEC.'//) 

W  R  I  T  E  (  6 , 7  7  ) 

77  FCRMAT (/5X, 'RMS  NOISE  LEVELS  ASSOCIATED  WITH  MEASUREMENT  VECTOR'/) 
WRITE  (6,7)(SIGMA(J),J=1,6) 

7  FCRMAT (/5X, • S I GM A ( R )  =  ' , F 10*2, 5X , »  S I GMA ( A Z ) = • , F 1 0. 6 , 5 X , ' SIGMA (EL)* • 
1, F10. 0,5X, 'SI GMA( ROOT )=• , F1C.2/5X, 'SIGMA ( AZOCT )=' ,F 1C.6 ,5X , ' SIGMA ( 
2ELCCT  )  =  ' , F 1 0 • 6 / ) 

W  R I T  E ( 6 ,  13)  MOATA 

13  FCRMAT (/5X,  'ONLY  THE  1  S T ' ,  I  3 ,  l X ,  ' OU AN T I T I E S  CF  THE  VECTCR'/ 

15X, 'CONSISTING  OF  R  ,  A Z  ,  E L  ,  R.CO T  ,  AZDO T  ,  ELCO  T  ,  1 . /BE  TA  ,  ARE  MEASURED') 
I F (CCCRCM- I . ) 780, 78C, 782 

780  WRITE (6, 781  ) 

781  FCRMAT ( /5X INI T  I  AL  STATE  VECTCR  READ  IN  RADAR-CENT EREC  RECTANGULA 
1R  COORDINATES'//) 

GC  TC  7  8  A 

782  WRI TE (fc, 783 ) 

783  FCRMAT ( / 5 X ,  '  INITIAL  STATE  VECTOR  READ  IN  RADAR-CENTERED  POLAR  COOR 
1CINATCS' //) 

784  WRI  TE(6,78)(CCVAR(J),J=1,7) 

78  FORMAT (/5X, 'RMS  NCISE  LEVELS  ASSOCIATED  WITH  INITIAL  STATE  VECTOR' 
1/7017*8//) 

WRITF (6, 799) ( FNCISE(J ) ,  J  =  1 , 7  ) 

799  FCRMAT ( /5X, 'NCISE  SAMPLES  AT  INITIAL  STATE ' /7D1 7* 8// ) 

81  WR  ITE (6, 12  )  ( ERRMAX ( JJ ) , JJ=1 , 7 ) 

12  FORMAT (/5X, 'MAXIMUM  INTEGRATION  CONVERGENCE  E R ROR S (  I N  RECTANGULAR 
1CCCRDINATES' / 7C17  *8/// ) 

L  =  2 
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C 

C  CATA  LCOP 

C 

CO  1000  NN  = 1 , NOAT A 
NN  =  NN 

IF(NN-1)21,20,21 
20  IF ( MOAT A ) 202 , 22, 202 
C 

C  ACO  NOISE  CF  RMS  LEVEL  COVAR  TO  NOMINAL  INITIAL  STATE 

C  ( NC I S  E  =  C •  FOR  ERROR  ANALYSIS  CASE) 

C  SET  INITIAL  STATE  ESTIMATE  =  INITIAL  NCISY  STATE 

C 

202  IF (COGRCM-1.  )  203, 203, 204 

203  XHAT (  1)  -X (  1  )  ♦  FNC  I  SE (  I) 

YHAT(l)  =Y(  1)+FNCISE(2) 

ZHATI 1)  =  Z I  1  )  +  FNC I SE ( 3  ) 

X  PF AT (  1  ) =  X  P (  1  )  +FNC  I  SE I  4 ) 

YPHAT (  1  ) = Y  P (  1)+FN0ISE(5) 

ZPHAT ( 1)=ZP(  1  ) +  FNO I SE ( 6 ) 

CALL  XYTCR( XHAT (  1  ) , YH AT ( I ) , ZHATI  1  )  ,XPHAT(  1  )  ,  YPHAT (  1 ) , ZPHAT ( 1 ) ,RHAT 
1(1), AH AT ( 1  )  , Eh AT (  1),RPFAT(  1  )  , APHAT {  1)  ,  E  PHA  T (  1  )  ) 

GC  TO  206 

204  RHAT(1)=RA(  1)+FNCISE(  1) 

AH  A  T (  1  )  —  A  Z (  1 ) +  FNC I SE I  2  ) 

EHAT (  1  )  =  E  L (  1 )  +  FNC  I  SE I 3) 

RPHAT (  1  )  =  RAP(  1)  +  FN0ISE(4) 

APHAT (  1)  =  AZP(  1)+FNGISE(5) 

E PH ATI  1)=ELP(  1)+FN0ISE(6) 

CALL  RTCXY ( RH AT (  1  ) , AHAT (  1  )  , EHA T (  1  )  , RPHA T (  1 )  ,  APHAT ( 1  )  , E  PH A  T ( 1 )  ,  XHAT 
1  (  1  ) , YHAT (  1  )  , ZHAT (  1  )  ,XPHAT(  1  )  , YPHAT (  1  ) , ZPHAT (  1  )  ) 

206  ESTATE! 7)  =  STATE  1(7 )+FN0ISE( 7) 

ESTATE ( 1 )  =  XH AT (  1  ) 

ES  T  AT  E ( 2 )  =  YHAT ( 1) 

ESTATE ( 3  )  ~ Z  HA  T  I 1 ) 

ESTATE (4  )  =  X  P  F  A  T (  1) 

ESTATE ( 5  )  =  YPH  A  1  (  1  ) 

ESTATE (6  )  =  Z  PH  A  T (  1  ) 

AL  PH  A (  1 ) =  ES  T  AT  E ( 7  ) 

205  BETAHI  1 )  =  1 . / E ST A TE I  7  ) 

T  AU  =  CE  LT 

C 

C  CCMPUTC  INITIAL  ERRORS  =  ( EST IMATE )- ( NGMI NAL  STATE) 

C 

CR (  1  )  =  RHAT (  1  )-RA(  1  ) 

CA(  1  )  = AH A  T I  1  ) - AZ (  1) 

CE (  1) =EHAT (  1  )  - EL (  1  ) 

C  R  P (  1  )  =  R  P  H  A  T (  1  )-RAP(  1  ) 

CAP!  1  )  =  A  P  F  A  T (  l  )  - AZ  P (  1) 

C  E  P  (  1 ) =  E  PH  A  T  (  1 ) - E  L  P  (  1) 

CB  (  1  )  =  BLTAH(  1 ) - B A  T  A (  1  ) 

CALPHt  l)=ESTATt( 7)— 1*/BETA 
CXI  1  )  =  X  H A  T (  1  )-X(  1 ) 

CY ( 1 )  =  Y  H AT ( 1  )-Y(  1  ) 

CZ (  1 )  =  Z  H A  T (  1 )-Z (  1  ) 

CXP (  1  )  =  X  PH  A  T (  1  ) - X  P (  1  ) 

C Y  P (  1) =YPHAT (  1  )  - Y  P (  1 ) 

CZPI 1 ) =  Z  PH AT (  1  )-ZP(  1) 

CC  TC  22 
C 

C  SET  UP  ARGUMENTS  FCR  TRAJGX  TO  INTEGRATE  NOMINAL  TRAJECTORY  TO 
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C  NEXT  PCINT 

C 

21  X 1  =  X2 
Y1  =  Y2 
Z1  =  Z2 

X0CT1=XDCT2 
YDGT  1  =  YDCT2 
Z  OCT  1  =  Z  DCT  2 
BET  A 1  =  BETA2 
T  AU  =  T 1  NT 
C 

C  0  PC AT  E  SUBSCRIPTS 

C 

22  K=NCC( NNf  IMAX ) 

N  X  T  =  K  +  1 

I NDEX=NX  T 
IF (K )23,23,24 

23  K -  IMAX 
C 

C  CALL  TRAJGX  IC  GENERATE  NLXT  LATA  PCINT  CF  NOMINAL  TR  A JbC  TORY 

C  IN  XYZ  CCLRCINATES 

C 

24  TIMEINXT )=TLAST+TAU 
T  L  AS  T= T I  RE (  INCEX  ) 

CALL  TKAJGXt  XI ,Y1 ,Z 1 ,XCUT1 f YDCT1 , Z LOT  1  ,  BE  T A  1  ,  TAU  , FI  NCR  ,  ERR  MAX  , 
1X2»Y2»Z2»XCL1 2 , Y CCT 2 , Z COT 2 , HI;  T A 2 » PH  IN A  I  ) 

X ( NXT )  =  X  2 
Y ( NXT  )  =Y2 
Z ( NX  T  )  =Z  2 
XP ( NXT  ) =XCC  T  2 
Y  P (NXT  )  - Y  CC  T  d 
ZP (NXI  )=ZCCT2 
BATAINxr )  =  8  E  T  A  2 

EEIGHI  <NXT)=C5C;KT  (  X2**2+Y2**2+  (  Z2  +  RE  )**2)-Rf 
C 

C  CCNVERT  TO  RACAR  PCLAR  COORDINATES 

C 

CALL  XYICK(X2,Y2,Z2 » XDCT 2 , YOU T 2 , ZDOT 2 , RA ( NX T )  f AZ (NXT )  , EL ( NXT )  »  K  A  P  ( 
1NXT)  f AZPINXT )  f  t  L  P ( NX  T  )  ) 

C 

C  CALCCLATt  ASPECT  ANCLE 

C 

ASP= ( X (NXT ) *XP ( NX  T ) *Y (NXT )*YP (NXT )+Z (NXT) *ZP( NXT )) / (RA (NXT) +DSCRT ( 
1XP(NXT)**2+YP(NXT)**2+ZP(NXT)**2)) 

ASPECT ( NXT ) =18C.-krCEG*DARC0S( ASP ) 

C 

C  WRITE  NEXT  NOMINAL  CATA  POINT 

C 

I  F  (  PR  NT  )  50 2  »  4  8  »  4  8 

48  WRITE(6,49) 

49  FORMAT (’  1’ ,5X,  ’NOMINAL  DATA  POINT’//) 

WRITE(G>5D)TIME(  I  N  D  EX  )  »  R  A  (  I N Oh X ) *  AZ (  I  NOE  X  )  f  CL ( I NOEX )  ,RAP  (  INDEX  )  f  AZ 
1P(  INDEX)  , ELP(  INCEX ) , X2 ,Y2# Z2 , XDGT2 » YDUT2 , ZDCT2 ,HE I GHT  (  INDEX)  »  B E T  A2 

50  FORMAT ( 5X ,» T  IMt  = ’ , F  10 . 4/ 5X ,  » R A  = • , F  1  5 . 2 » 3 X ,  » A Z  =  ’  ,  F  1  0  •  6 , 3  X  ,  ’  E  L  =’ 

l,F10.6,3X,’RAP=’fFlC.2»3X*’AZP=’tFlC.6,3X,’FLP=’,FlD.6/5Xf’X  =’*F 

210.2,3X,’Y  =’  ,F  10.2, 3X, ’Z  = * , F 10. 2 1 3X, ’ XP  =’»F10.2»3X»’YP  =*,F10 

3«2f3Xf ’ZP  =’ , FIG. 2/ 5X, ’HEIGHT  = », 5X , F 1 5 • 2 , 20X , ’ BE T A = ’ , 5X , F 1 D. 2 // ) 

WRITE  (6f500)ASPECT  (NXT  ) 

C 

C  WRITE  TRANSITION  MATRIX  FOR  NOMINAL  DATA  PCINT 
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501  V>RITE(6,5l)<<PhlMAT(JJ,KK),KK=l,7),JJ=I,7) 

51  F  C  R  M  A  T  (5Xf  'TRANSITION  MATRIX  PH  1  •  /  (  70  1  7 . 8  )  ) 

502  IF(MDATA)5800,25,24C0 
C 

C  ERRCR  ANALYSIS 

C 

5  80C  S  T  A  T  C  P  (  I  )  ■=  X  2 
ST ATtP( 2  )=Y2 
STATEP( 3  )  =  Z  2 
S  T  A  TE  P ( 4 ) =X  CC  T  2 
STATLP(5)=YCCT2 
STATLP(6  )  =  ZL'CT2 
S  T  A  T  E  P  (  7 )  =  1 •/ BE  T  A2 

CALL  ESTMAI(STATEP,5TArtP,PhlMAT,ESTATE,STD£VX,STDEVK) 

C 

C  STCRE  EXPECTEC  RMS  tRRCRS 

C 

SICR  (NXT  )  =  STLEVK(  I  ) 

SIGA  (NXT  )=STCEVR(2) 

SIGE  (  N  X  T  )  =  S  T  C  EVR ( 3) 

SICRP(NXT )  =  S 1 C E VR ( 4 ) 

S  I  G  A  P ( NX  T )  =  S  T  D  E  V  R  ( 5) 

S  l  G  E  P.  (  N  X  T  )=STLEVR(fc) 

SIGX  (NXT  )  =  S  T  C  t  V  X  (  1 ) 

SIGY  (NXT  )  =  S  TCEVX ( 2 ) 

SIGZ  (  NXT  )  =STLFVX (  i) 

S  IGXP  (  NXT  )  =STi:tVX  (  4  ) 

SICYP(NXI  )=STCEVX( 5  ) 

SIG/P(NXT  )-STCLVX(6) 

SIGALP(\XT  )  - S  T  T  E VX (  7) 

GC  TC  25 
C 

C  TRACKING  A  \L  ESTIMATION. 

C 

C  ACL  NL1SE  LF  *<f^  S  LlVLL  SIGMA  TL-  OBTAIN  SIBILATED  MEASUREMENT 

C 

2A0C  CC  241  J  =  L  »  4 

24  1  CALL  GAUSS \ (  I  , CM  I  Sc ( J  )  »  S 1 ( N A ( J )  ) 

RGN  (  NX  T  )  =?U(  NX  f  )  +CNL1  SL  (  1  ) 

AZN ( NX  T )  =  A/ ( NXT >*GM.  1 S  t  (2) 

ELN  (  N  X  I  )  Hi  (NX  I  )  4L,\l  1st  (  3  ) 

RAPN ( NX  T  )=RAP (NX  T  )  4GNU1SL ( 4) 

STATFM  1  )  =  R G N  (  N X  T  ) 

STATlF  (2)  =  AZ\(NXT  ) 

STATEN (  3 )=LLN (NXT  ) 

S  T  A  1  1  M ( 4 ) =R APN (NX  1  ) 

IF  (PRM  )  5  4  t  5  7  t  5  7 
C 

C  PRINT  LUT  SIMULATED  NOISY  MEASUREMENT 

C 

5  7  VnR  I  T  E  (  G,5  8  )  T  I  ML  (  NX  I  )  f  RGN  (  NXT  )  ,  AZN  (  NX  T  )  ,  ELN  (  NXT  )  ,  RAPN  (  NX  T) 

58  rCRFAT(/5X,»NCISY  C  A  T  A  •  /  5X  ,  *  T  I  ME  =  9  ,  F  I  C  .  4 , 5X  ,  *  RON  =  *  ,  I  1  C  .  2 , 3X  ,  •  AZN= 
I  •  ,F  lO.b, 3X , * ELN=* , F 10.6, 3X,  *RAPN= • ,F 10.2// ) 

C 

C  CALL  TrAJGX  FL  CCTAIN  NEXT  PREDICTED  DATA  PC1NT 

C 

59  CALL  TR A JGX( ES  T ATE (  l  )  , EST ATE ( 2  )  , ESTATE ( 3) ,ESTA  TE (4 )  tES  TATE ( 5) f ESTA 
1  TE  (6  )  ,  fclTAMK  )  •  TAU,  I  I  NCR,  EKRMAX,  STATEP  (  1)  ,  STATEPI2)  ,STATfcP(  3)  ,  STAT 
2EP(4)  ,STATEP(5) »  S  T  A  1  f  P  (  6  ) ,  PBETA, PRCC ) 
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C 

C  CCNVERT  NEXT  PREDICTED  DATA  POINT  TO  RADAR  POLAR  COORDINATES 

C 

STATEP( 7)  =  I./PGETA 
IF ( PRNT) 5702*5700?  57CO 

5700  CALL  XYTCR(STATEP(l),STATEP(?),STATEP(3),STATEP(4),STATEP(5)tSTATE 
IF (6 ) , PRA,PAZ , PEL,PRAP»PAZP,PELP ) 

FH=CSCRT ( ST  ATEP(  I )**2+STATEP(2)**2+(STATEP(  3 ) +  RE ) **2 ) -RE 
RRITE(6,57C) 

570  FCRMAT ( /5X ,* PREDICTED  STATE  BASED  ON  PAST  DATA  C  NL Y ' ) 

RR  I  TE  (  6, 50  )  T IMF (NXT ) , PR  A, PAZ, PEL ,PRAP,PAZP, PELP* STAIEP (  I  )  ,  STATEP(2 

1) tSTATEP(3),STATEP(4),STATEP(5),STATEP(6),PH,PBETA 
C 

C  CALL  ESTMAT  TL  COMBINE  SIMULATED  MEASUREMENT  WITH  PREDICTED  POINT 

C  TC  CBIAIN  FINAL  ESTIMATE  OF  NEXT  DATA  POINT 

C 

5702  CALL  ESTMAT ( STATEM, STATEP* PROD, ESTATE, S TD E V X , S TDE VR  ) 

C 

C  STCRF  tXPECTEC  RMS  ERRORS 

C 

SIGR  ( NX  T ) =S  TC  EVR (  I  ) 

S IGA  (NXT  )  =S  T  C  E  VR ( 2  ) 

SIGE  ( NXT ) =STDEVR ( 3) 

SICRP(NXT)=SIDEVR(4) 

SIGAP(NXT  )  =  S  T  C  E V  R ( 5  ) 

SICtP(NXT )=SICEVR(6) 

SIGX  ( NXT  )  =  S  T  C  E  V  X (  I  ) 

SIGY  (NXT)=STLEVX(2) 

SIC/  (NXT)-STCEVX(3) 

SIGXP(NXT)=SIDEVX(4) 

SIGYP(NXT)=STDEVX( 5) 

SIGZP(NXT)^STCEVXU) 

S1CALPI NXT  )  =  S  T  D  E  V  X (  /  ) 

579  CONTINUE 
C 

C  CONVERT  ESTIMATED  STATE  TC  RADAR  POLAR  COORDINATES 

C  STORE  LSTIMATEC  STATE  VECTOR  IN  OUTPUT  ARRAYS 

C 

5793  CALL  X Y T CR ( E S T A T E ( I) , E S T ATE ( 2 ) , L ST A TE ( 3 ) , L S TA TE ( A ) , E S T ATE ( 5 ) , E ST AT 
IE (6 ) v KHAT (NXT ) 9 AHATCNXT ) tEHAT(NXT ) v RPHAT (NXI)  ♦  A  PHAT  (NXT  )  f  EPHAT  (NXT 
2  )  ) 

XHAT ( NX  T  )  =  ES  T  ATE (  I  ) 

YhAT ( NXT  )=C  ST  A  I  L ( 2  ) 

ZHAT (NXT  )  =  E  S  r  A  r  E ( 3) 

XPhAT ( NXT  RESTATE ( 4  ) 

YPFAT (NXT)=ESTATE(5) 

ZPFAT  (  NXT  RESTATE  (  6  ) 

ALPHA(NXT)=ESf ATE(  7) 

6  E  T  AH ( NX  I  )  =  1. /ESTATE (  7) 

HNXT=CSCRT ( ESTATE (  1)**2+ESTATE(2)**2+(ESTATE(3)+RE) **2  )- RE 
IF (PRNT ) 581* 5794*5794 
C 

C  PRINT  CUT  ESTIMATED  STATE 

C 

5794  WRITE(fc,5HC) 

580  FORMAT (  /5X  * ’ESTIMATED  POINT*/) 

WK ITE(6,50) T IME(NXT ) ,RHAT(NXT ) » AHA T( NXT ) »EHAT (NXT ) * RPHAT(NXT ) , APHA 
IT (NXT ) *  h  P  F  A  T (NXT )  *  t  S  T  A  T  t  (  1  )  »  E  S  T  A  T  E  (  2  ) *  E  S  T  A  T  E  ( 3)  fESTATE (4)  »FSTAIE (5 

2) ,ESTATE(6)»FNXr,HETAF(NXT) 
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C  COMPUTE  C I  FFERENCES  BETWEEN  ESTIMATE  ANO  NOMINAL  DATA  POINT 

C 

581  CR(NXT)=RHAT(N*T)-RA(NXT) 

CA(NXT)=AHAT (NXT)-AZ(NXT) 

CE(NXT)=EHAT (NXT)-EL(NXT) 

CRP( NXT )=RPHAT (NXT  )-RAP { NX  T  ) 

CAP ( NXT ) = APHAT (NXT  )-AZP(NXT) 

CEP ( NXT )=E PH AT (NXT  )-ELP(NXT  ) 

CB(NXT)=8ETAH(NXT)-BATA(NXT) 

CALPH(NXT)=ESTATE(7)-i./BATA(NXT) 

CX (NXT )  =  X  H  A  T (NXT)-X(NXT) 

CY (NXT )  =  YHAT ( NXT  )  —  Y ( NXT  ) 

CZ (NXT )  =  Z  HA  T (NXT )  —  Z (NXT  ) 

CXP(NXT)=XPhAT(NXT)-XP(NXT) 

CYP(NXT )  =  Y  Ph  AT (NXT  )-YP(NXT  ) 

CZP(NXT)=ZPHAT (NXT)-ZP(NXT) 

C 

C  TEST  FOR  LAST  CATA  POINT  OF  RUN 

C 

25  1F(NN-NDATA)27, 26,26 

26  KM  AX  = INDEX 
GC  TC  AO 

27  IF(INnEX-IMAX)I0CC,28,26 

28  KM  AX  =  I  M  A  X 
C 

C  WRITE  OUTPUT 

C 

AO  WRITE (6, 10 )  ( LABEL (  1  )  , 1  =  I  , 18) 

WR I Tb ( 6  »  4 l  ) 

A I  FCRMAT (/5X, 'NOMINAL  TRAJECTORY  IN  RADAR  COORDINATES1// 

1  3X  f 1 T I  ME ' t  8X ,  'RANGE ' , 7X  , ' RACCT • , 7X , • A Z I M f , 8 X , ' A  Z  DOT • , 7X 

2  , '  ELEV  f  »  8X  f  'ELDCT' ,  7X  , 'HEIGHT', 6X,  'BETA'//) 

VnRITE(6,42  )  ( T IMb(KK) »RA(KK )  ,  R  A  P  (  K  K  ) ,  A  Z  (  K  K  ) ,  A  Z  P  (  K  K  ) ,  E  L  (  K  K  )  ,  E  L  P  (  KK)  , 
IFEIGHT (KK),BATA(KK),KK=1,KMAX) 

A  2  FCRM  A  T (  3X,FI0.3,2X,FI0,2,X,F10 

2.2,2X,F10.6,2X,F10.0,2X,F10.6,2X,F10.6,2X,FiC.?,2X,Fl0.2) 

W  R  I  T  E ( 6  *  10)  ( LABEL (  I  ),  1  =  1,  18) 

WRITE(6,AI0) 

A  l  0  FORMAT (  5  X  , 'NOMINAL  TRAJECTORY  IN  XYZ  COORDINATES'//) 

WRI TF(6, Ail ) 

All  FORMAT  ( 3  X  * 'TIME'  t8X» 'X'  ,  ilX,  '  Y ' ,11X,  'Z '  »  I  IX,  '  XDGT »  ,  8X  ,  '  YDOT ' f 8X, ' Z 
1CCT'  ,8Xf  '  fi  E  T  A  ' ,8X,  'ASPECT  ANGLE ' //) 
WRITE(6,4l2)(TIME(J),X(J),Y(J),Z(J),XP(J),YP(J),ZP(J),BATA(J),ASPE 
1C  T ( J  )  ,  J=i,KMAX) 

A  l  2  FCRMAT(3X»FlG.3»2X,FlO*2,2X,FlO.2,2X,FlO.2,2X,FlO.2,2X,F10.2,2X»Fl 
IC.2,2X»F10.2,2X,F1C.2) 

IF  (MEAT  A )900,  IC0C,A2C 
A20  WKI  IE (6, 1C) (LABEL (  I  ) ,  1  =  1,  IP ) 

WRI TF (6, A3) 

A3  FORMAT ( 5X, 'MEASUREMENT  DATA'/ /6X, 'TIME', 8X, 'RANGE ’ , 7X , • RADOT ' , 17X , 
l ’ AZ IM '  ,  20X  ,  '  ELEV'//  ) 

WRI TE(6»AA)  (TIMFtKK ) ,RGN(KK  )  »RAPN(KK) ,AZN(KK)  ,  L  L  N  (  K  K  )  ,KK  =  L  ,KMAX) 

AA  FORMAT (3X,F10.3,2X,F10.2»2X»F10.2, 1AX,FI0.6,1AX,F1C.6) 

WRITF  (6,10)  (LABEL!  I  ),  1  =  1,  18) 

W R  I  T L ( 6 , 60 ) 

60  FORMAT  (  t>X  ,  'ESTIMATEL  VALLES'//) 

WRlTF(b,6l ) 

61  FORMAT ( 3X, '  T I  ME'  , 8X, 'RANGE ' , 7X,  'RADOT  ',7X,'AZIM',8X,»AZDCT»,7X,'EL 
IEV'  ,8X,  'FLCOT'  ,7X,  ’BETA',8X,  'ALPHA'//) 

WRITE(6,62)  (  TIME{ J),RHAT( J),RPHAT( J),AFAT( J),APHAT( J)  ,  EH A  T ( J )  , E  PHA 
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IT ( J ) , 8  ET  AH ( J ) , ALPHA ( J ) , J=1,KMAX) 

62  FORMAT ( 3X f F 10 . 3 , 2X , F 1 0. 2 , 2X , F 10 . 2 , 2X f F 1 0. 6 , 2X f F 10. 6 , 2 X , F 1 0 . 6 , 2 X , F 1 
10.6,2X,F10.2,2X,F12.8) 

ViRITE(6»10)  ( LABEL  (  I  ),  1  =  1,  18  ) 
hR I T  E ( 6 , 60  ) 

WR  I TE  (  6.?  4  13  ) 

A  13  FORMAT ( 3X, *T IME* ,8X, *X» , HXf 1 Y* « 11X, • Z» • 1 IX, 'XDOT' ,  8Xf • YDOT • ,8X, • Z 
1 CCT 1 ,  8  X ,  1 8ET  A  1 »  8X  » 'ALPHA*//) 

hRITE(6,4l4) ( T  I  ME ( J ) »XHAT( J) ,YHATC  J) ,ZHAT( J)  , XPHA T ( J ) , YPH A T ( J )  ,ZPH 
1  AT  C  J  )  , BET AH  C  J ) f ALPHA ( J ) , J=1,KMAX) 

A  1 A  FORMAT (3X,F10.3f2X,Fl0.2f2X,F10.2f2XfFl0.2f2XfFl0.2,2X,F10.2,2XfFl 
10.2,2X, F10.2,2X, F12.8) 
fcRITE  (6*  10  )  (LABEL  C  I)«lsl«18) 
hRITE(6,63) 

63  FORMAT ( bX,  •( ERRORS  )  =  ( ESTIMATED  VAL UE S ) - ( NOM I NAL  VALUES)'//) 
h R  I  TE (6, 61 ) 

Wi  R  I  TE  C  6,62  )  (  TIME  (  J  )  ,DR(  J  ) ,DRP( J  )  «DA(  J  )  •  DAP  (  J  )  9  DE  (  J  )  «DE  P  (  J  )  «D8  (  J  )  «  D 
1ALPH(J)  ,J=1,KMAX) 

V«R  I T  E  (  6 1  10)  (LABELt  I  )«  I-l»  18) 
bRITE(6,63  ) 

V«RITE(6v  A13) 

hRITE(6,414)(TIME(J),DX(J),CY(J)  *  DZ  (  J  )  *  DXP  (  J  )  ,  D YP (J ) , DZP ( J )  ,DB( J) , 
1C AL  PH ( J ) fJ— 1 fKMAX ) 

900  V<RITE(6, 10)  (LABELt  I), I =1,18) 
bRITE (6,901 ) 

901  FCRMAT( 5X, 'EXPECTED  RMS  ERRORS'//) 

ViRITE(6,6l  ) 

WiR  I  T  E  C  6,902)  (  T  I  ME  (  J  )  ,  S  I GR  (  J  )  ,  S I  GRP  (  J  )  »SIGA(J)tSIGAP  (J),SIGE(J),SIG 
1 EP ( J ) f  S I  GAL P ( J ) ,  J=L,KMAX) 

902  FORMAT ( 3 X , F 10 . 3 f 2X , F 1 0 . 3 , 2X f F 10 . 2 , 2X , F  10. 6 , 2X f F 1 0. 6 , 2 X , F 1 0. 6 f 2 X , F 1 
1C. 6,  1AX, F12.8 ) 

ViR  I T  E  (  6  ,  10  )  ( LABEL (  I  ) ,  I  —  1 »  18) 

ViR  I  T  E  (  6  f  90  1  ) 

ViRITE(6,413  ) 

hRITE(6,903)(TIME(J)fSIGX{J)»SIGY(J)fSIGZ(J) , SIGXP ( J ) ,SIGYP(J),SIG 
1ZP(J) *SIGALP(J)*J=L*KMAX) 

903  FORMAT ( JX, F 10.3, 2X, F 10. 2, 2X, F 10. 2, 2X,F 10.2, 2X, FI  0.2, 2X,F 10. 2,2 X, FI 
10.2, 1AX,F12.8) 

L=  1 

1000  CONTINUE 

1001  CO  TO  1 

9000  RETURN 

ENC 
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C  MAIN  PROGRAM 

IMPLICIT  REAL*8( A-H,0-Z ) 

CCMMCN  / ACCM/CCVAR( 7) , S  I GM A ( 7 ) / F COM /C OCRD , DL A T , PRNT / 1 C CM / K L AMP , MD A 
1 T  A , NN 

Cl  MENS  I CN  GNCI SE ( 7  )  , RGN ( 54  )  , AZN ( 54 ) , E L N ( 5 A )  , RA PN t 5 A ) , B  E  TAH ( 54 )  , 

1  RhAT( 54 ) , Ah AT ( 54 ) , EFAT  t  54 )  ,RPHAT ( 54 ) , APHAT ( 5  4 ) , E PH AT ( 54 ) ,  STAT 

2EM(7),STATEP(7), PH  I MAT (7,7) ,FNOISE (7) , ALPHA ( 54) ,D AL  PH ( 54 ) 

Cl  MENS  I CN  PHINV( 7,7),PROC( 7,7) , ESTATE ( 7) , STATE  I  ( 7 ) , X ( 54 )  ,  V ( 54 )  , Z ( 5 
14)  ,XP(54)  ,YP(54),ZP(54),CX(54),DY( 54) ,DZ(54) ,DXP(54)  ,DYP(54)  ,DZP(5 
24  )  , XHAT ( 54  )  , YhAT ( 54  )  ,ZHAT ( 54 ) , XPHAT ( 54 ) , YPHAT ( 54 ) , ZPHAT (54 ), ASPECT 
3(54),SIGR(54),SIGA(54),SIGE(54),SIGRP(54),SIGAP(54),SIGEP(54),SIGX 
4 ( 54 ) ,S I G Y ( 54 ) , S1GZ (54 ) ,S IGXP( 54 ), SI  GYP (54 ) ,SIGZP (54 ) , SIGALP(54  ) 

Cl  MENS  I CN  GN(7),LABEL(18),RA(54),AZ(54),EL(54),  A ZP ( 54 )  , E LP ( 5 

14  )  ,FEI GHT ( 54 ) , T I  ME ( 54 ) ,RAP ( 54 ) , B AT  A ( 54 ) , ERR MAX ( 7 )  , S TOE VR ( 7 )  ,STDEVX 
1(7) 

Cl  MENS  I CN  DR(54),DE(54),DA(54),DRP(54),DAP(54),DEP(54),DB(54) 

C 

C  CALL  CENS  TC  SET  UP  ATMOSPHERIC  DENSITY  TABLCS 

C 

CALL  DENS 
C 

C  READ  INPUT  PARAMETERS  AND  PRINT  ON  LISTING 

C 

1  PEAC (5 ,2  »ENC  =  9C0C )  ( L ABEL ( I  )  ,  I  =  1 ,  1 8  ) 

2  FORMAT  ( 18A4 ) 

READ (5 ,4 )TZERC,T INT , T INCK»DLAT ,PRNT , NO AT  A  , KLAMP , MDA T A 
4  FORMAT ( 4F  10. 3, F5. 3, 31 5 ) 

RE AC (5, 3) (SIGMA( J), J= 1 , 6 ) , CCORD 

3  FORMAT ( 7F10.3) 

RE AC ( 5 , 3 ) (ERRMAX(JJ),JJ=1»7) 

9  I M  A  X  =  50 

CCCRCM=DA6S( CCCRC  ) 

RTCEG=18C./3. 14159265 
C 

C  L  A  Pfc  L  =  ICENTIFYINO  COMMENTS  TO  APPEAR  WITH  DATA  PRINT-CUT 

C  TZERC=  INITIAL  TIME 

C  TINT=INTERVAL  bETWEEN  DESIRED  DATA  POINTS  (SEC*) 

C  T INCR=INTEGRAT ICN  STEP  SIZE  (SEC.) 

C  CL AT  -  LATITUDE  OF  RADAR  SITE  (IN  DEGRFLS) 

C  NDATA= NUMBER  CF  CESIRED  DATA  POINTS 

C  PRNT=PPINT-CUT  SELECTOR 

C  PRNT  =  - 1  •  MINIMUM  PRINT-OUT,  TABULATED  SLMMARY  ONLY 

C  PR  NT  =  0.  PRINT-CUT  CF  COVARIANCE  MATRICES  ♦  TABULATED  SUMMARY 

C  PR  N  T=  1.  FULL  PRINT-GUT 

C  KLAMP=  ME  MCRY  OF  AL GOR  I  T HM , E XPR t S S E D  AS  NUMBER  OF  DATA  POINTS 

C  MCATA^MCCE  StLECTLR 

C  MCATA  GREATER  THAN  C.  TRAJECTORY  PARAMETER  ESTIMATION 

C  CCCRC  =  CCCRCINATt  AND  UNIT  SELECTOR 

C  CL'CRC  =  -2.  POLAR  COORDINATES,  METRIC  UNITS 

C  CLCRC=+2.  POLAR  COORDINATES,  ENGLISH  UNITS 

C  SIGMA  =  RMS  NOISE  COMPONENTS  GF  MEASLREMFNT  VECTOR 

C  ERRMAX  =  TRAJECTORY  INTEGRATION  CONVERGENCE  ERRORS  (IN  REC- 

C  TANGULAR  COORDINATES) 

C 

WRITE  €6, 1C) ( LABEL ( I ), I - 1 , IB) 

LO  FORMAT (  1  1  1  ,2CX, ’OUTPUT  L I S T I N U f / 1 8 A 4 / / ) 

W  R  I  T  E  (  6  ,  11CJCLAT ,NCATA,TINT»T I  NCR 

1  LO  FORMAT (  / 5X , T  RADAR  L AT  I TUDE  *  , 3X , F  1C . 5 , 2X ,  * DEGREE S ’/ 5X , I  6 , 5X , * 

1 C  AT  A  PLINTS  SPACED • , 3X ,  F10.6  ,2X,fSEC.  APART,  INTEGRATION  STEP’, 2 
2X,F10.6,2X, *  S  E  C  .  T  /  /  ) 
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IF(CCCRD) 14, 16, 16 

14  RE=2.0925738C7/3.2808333 
V*  R  I  T  E  (  6  ,  15  ) 

15  FCRMAT (/5X, ’METRIC  UN  I T S ( METERS , K I UOGRAM S , R AD  I ANS , SECOND S )  USED  TH 
1 RCUGHOU  T  PROGRAM*//) 

GC  TO  18 

16  RE=2.0925738C7 
KR I TE ( 6 , 17) 

17  FCRMAT ( /5X,  *  ENGL ISH  UN  I TS( F EET , POUNDS , RAD  I ANS , SECONDS )  USED  THROUG 
lhCUT  PROGRAM*//) 

18  CC  19  J=l,7 

19  STATEM(J)=0. 

75  IF ( KL AMP ) 8  1 , 8 1 , 75  1 

751  l*R  I  T E  (  6 , 752  )  KLAMP 

752  FCRMAT ( /5X MEMORY  TIME  =*,I6,'  DATA  POINTS'//) 

81  b  R I TE ( 6 , 12) ( ERRMAXI JJ ) , JJ=1, 7) 

12  FCRMAT (/5X,* MAXIMUM  INTEGRATION  CONVERGENCE  ERRORS ( IN  RECTANGULAR 
1CCCRCINATES* /7C17.8///  ) 

L=  2 

L  A  S  T  =  0 
C 

C  CATA  L CC P 

C 

CC  1000  NN  =  1  , N  C  AT  A 
NN  =  NN 
NNP  1  =  NN  +  1 
C 

C  TEST  FCR  1ST  TIME  THROUGH 

C 

IF (NN-1 ) 2 1 , 2C , 2  1 
C 

C  R E AC  INPUT  DATA  CARCS 

C 

20  RE  AC ( 5 , 30  )  T I  ME ( 1 ) , RGN ( i ) , AZN ( 1 ) , ELN ( 1 ) , RAPN ( 1 ) 

30  FCRMAT (U15.2»4C15.6) 

I F  C  T I  ME ( 1 )-TZER0)20,200,20G 
C 

C  SET  INITIAL  STATE  ESTIMATE  =  INITIAL  NOISY  STATE 

C 

200  RHAT (  1  )  =  RGN (  1  ) 

AHAT (  1  )  =  A  Z  N  (  1  ) 

EH  A  T  (  1  )  =ELM  1  ) 

20  1  R  E  A  C ( 5 , 30  )  TIMEI2 ),RGN(2) ,AZN(2) , ELN ( 2 ) , R A PN ( 2 ) 

C  T  =  T  I  ME  (  2  )  -  T  I  ME  (  1) 

IF (CT-TINT )201, 202*202 

202  APHAT  (  1  )  =  (  AZM  2  )- AZN  (  1  )  )/CT 
EPHAT(1)=(ELN(2)-ELN(1))/DT 
CC V  AR (  1  ) =  S  I  GM  A (  1) 

CCVAR( 2)=SIGMA( 2  ) 

CCV AR ( 3 ) =S I GMA ( 3) 

CCVAR(5)=1.414*S  IGMA( 2)/CT 
CCVAR(6)=1.4i4*S IGMAI 3  )  /CT 
CC V  AR ( 7  )  =  i  • 

IF (MCATA-3)2C3,2C4,205 

203  VvRlTE(6,21C) 

210  FCRMAT ( //5X, ' ***  THIS  MAIN  PROGRAM  REQUIRES  MDATA  =  3  CR  4 ,  QUIT') 
RETURN 

204  RPHAT  (  1  )  =  (RGN(  2  )-RGM  1  )  )/CT 
CCVAR(4 )= 1.4 14*SIGMA(1) /CT 
GC  T C  206 
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205  RPHAT (  1 ) =R A PN (  1) 

CCVAR(A)=SIGMA(A) 

206  EST ATE ( 7  )= 1 .C-4 

EET AH (  1  )  =  1  •  /  EST ATE ( 7  ) 

C 

C  PRINT  HEADER  PAGE  CF  OUTPUT  LISTING 

C 

WR I TE ( 6  *  13)  MDATA 

13  FORMAT ( /  5  X  , *CNLY  THE  1  ST • , I  3  *  IX  * ' QUANT  I TI  E S  CF  THE  STATE  VECTOR'/ 
15X, 'CONS ISTING  OF  R , AZ , EL  * RDOT * AZDOT, ELDOT ,  1 . /BE TA ,  ARE  MEASURED') 
WRITE(6,77) 

77  FORMAT ( / 5X  * ' RMS  NOISE  LEVELS  ASSOCIATED  WITH  MEASUREMENT  VECTOR'/) 
WRITE  (6*7 ) ( SIGMA (J  )  *  J  =  1  *  6  ) 

7  FORMAT ( /5X, • SIGMA ( R )=' *  F 10 • 2  *  5X  * 'SIGMA ( AZ )= • , F 10. 6 , 5X, 1 S I GMA ( E L ) = • 
lrF10.6*5X,  ' S I GMA ( R  DOT )  =  ' ,F10.2/5X* • S I GMA ( A ZCOT )  =  • , F 1 C . 6 , 5 X , ' SIGMA ( 
2 EL  D CT )  =  '  *  F I  0 • 6/ ) 

WRITE(6*78  ) 

78  FORMAT ( / 5X ,  '  RMS  NOISE  LEVELS  ASSOCIATED  WITH  INITIAL  STATC  VECTOR' 
1/  ) 

WRITE ( 6*7 ) ( CCVARC J  )  . J=l. 6) 

WRITE(6,79)  COV  AR ( 7) 

79  FORMAT (5X,'SIGMA(  1/BETA)='*F10.3//) 

C 

C  SET  UP  INITIAL  STATE  IN  BOTH  RADAR  POLAR  AND  XYZ  COORDINATES 

C 

CALL  RT  CXY ( RH AT (  1  )  * AHAT (  1  )  , EH A T (  1 ) , RPHA T (  I  )  ,  A PH AT ( 1 )  , EPHAT (  1  )  , EST A 
ITE(l)  *ESTATE(2)i  EST  ATE  (3),  ESTATES  RESTATE  (5)  *L  STATE  (6)) 

HNXT  =  DSCRT (ESTATE!  1  )**2+ESTATE (2)**2+(ESTATE( 3 ) +  RE ) ** 2 ) -RE 
XHA T ( 1 ) =ES  T  ATE ( 1 ) 

YHAT ( 1 )=ESTATE(2) 

ZhAT ( 1)=ESTATE( 3) 

XPHAT (  1)-ESTATE(4) 

Y  PH A  T (  1)=ESTATE(5) 

Z  PH  A  T (  1  )  =  ES  T  A  T  E  (  6 ) 

AL  PHA (  I  ) =  E  S  T  AT  E ( 7) 

HEIGHT ( 1  )  =HNX  T 
N  X  T=  I 

WR  I  TE ( 6  *  49 ) NN 

WRITE(6*50)TIME(NXT  ),RHAT (NXT  )  , AHAT (NXT ) , EHAT(NXT)*R PH AT( NX T),APH A 
lT(NXT)*EPHAT(NXT)*ESTATE(L)*ESTATE(2)*ESTATE(3),ESTATE(4),tSTATE(5 
2  )  • ESTATE ( 6  )  , HNXT *  BET  AH ( NX T  ) 

K=  1 
NXT  =  2 
I NC  EX  =  2 
GC  TO  31 
C 

C  UPDATE  SUBSCRIPTS 

C 

21  K  =  MC  D ( NN ,  IMAX ) 

NXT^K* I 

INDEX=NXT 

IF(K)23,23,24 

23  K= IMAX 
C 

C  READ  NEXT  DATA  POINT 

C 

24  READ (5, 30*  END  =  80C0 ) T I  ME ( NX T ) , RGN ( NXT ),AZN(NXT),ELN(NXT),RAPN(NXT) 
DT  =  TIME(NXT ) - T  L  A  S  T 

IF(DT  —  TINT >24*31* 31 
31  TLAST=TIME(NXT) 
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T  A  U  =  D  T 
C 

C  CALL  TRAJGX  TC  CBTAIN  NEXT  PREDICTED  DATA  PCINT 

C 

CALL  TRAJGX <  ESTATE! 1 ), ESTATE! 2 ) , ESTATE! 3) ,  ESTATE (4) ,  E  S  T  A  T  E !  5  ) ,  E  S  T  A 
L  T  E  !  6 ) ,  BETAH!K),TAU,TINCR,ERRMAX,STATEP( 1), STATEP (2)  ,STATEP!3) ,  STAT 
2EP!4  NSTATEP! 5) ,STATEP! 6) , PBETA,PRCD  ) 

C 

C  CCNVERT  NEXT  PREDICTED  DATA  POINT  TC  RADAR  POLAR  COCRD I  NATES 

C 

57  00  CALL  XYTCR!STATEP<  1 ), STATEP < 2 ), STATEP ( 3  ), STATE P ( 4 ) , S TA TE P ! 5 )  , S T AT E 
IP!  6 )  ,  PR A, PAZ, PEL, PR AP , P AZP , P EL P  ) 

PH=D SORT (STATEP! I )**2+STATEP(2)**2+! STATEP! 3)+RE )**2)-RE 
STATEP! 7 )  =  1 • / PBET  A 
STATEP! 1 )  =  RGN!NXT  ) 

STATEH!2)=AZN(NXT) 

STATEP ! 3 ) =ELN  <  NX  T  ) 

S  T  A  T  EP  !  4  )  =  R  A  PN  !  NX  T  ) 

IFtPRNT )5702, 569,569 
C 

C  PRINT  CUT  NCISY  PEASUREMENT 

C  PRINT  CUT  PREDICTED  STATE 

C 

569  R  I  T  E ( 6 , 49 ) N N  P  1 

49  FCRPAT ! • 1* ,5X, 15, •  TE  CATA  POINT1//) 

ERITE (6,58)  TIPE(NXT),RGN(NXT),AZN(NXT) , E LN ( NX T ) , RA PN C NX T ) 

58  FCRPAT ( /  5X » ’NCISY  C AT  A • / 5X ,  • T I  ME  -  '  *  F 1C*  4 , 5X , '  RGN= •  »F1C*2,3X» '  AZN  = 
l',F10*6,3X,»ELN=’,F10*6,3X,  ’RAPN=' ,F 10.2/  /  ) 

V>RITE(6,57C) 

570  FCRPAT(/5X, 'PREDICTED  STATE  BASED  ON  PAST  DATA  LNL Y ' ) 

WRITE!6,50)TIME(NXT),PRA,PAZ,PEL,PRAP,PAZP, PEL P , STA TE P ( 1 )  ,  STATEP!  2 

1),STATEP!3),STATEP!4),STATEP!5),STATEP(6),PH,PBETA 
IF { PRNT ) 5702, 5702, 5701 
C 

C  PRINT  CUT  TRANSITION  MATRIX 

C 

5701  fcRITE(6,51) ! (PROD! JJ,KK), KK= 1,7), JJ= 1,7) 

51  ECRPAT(/5X,  'TRANSITION  M A T R I  X  * / ! 7D  1  7 . 8  )  ) 

C 

C  CALL  ESTPAT  TC  COMBINE  MEASURE PFNT  M  TH  PREDICTION  TC  OBTAIN 

C  ESTIMATE  OF  STATE  VECTOR 

C 

5702  CALL  ES T P A T { S T A T E P , ST  A T t P , P RC C , E S T A TE , S TDE V X , S T DE VK ) 

C 

C  STCRE  EXPECTED  RPS  ERRORS 

C 

SIGR  ( NX  T )=STUEVR (  1  ) 

SIGA  (NXT)=STDEVP(2) 

SIGE  INXT )  =  S  T  D  E VR ( 3) 

S I CRP ! NX  T )  =  STCEVR( 4  ) 

SIGAP! NXT )  =  S  T  D  E V  R ( 5) 

S1GEPCNXT )=STDEVR(6) 

SIGX  ! NX  T )  =  S  TD  EVX (  1) 

SIGY  ( NXT )  =  S  TC  E VX ( 2  ) 

SIGZ  ! NX T )  =  S T D E V X ( 3  ) 

SIGXP(NXT )  =  S  T  C  EV  X (4  ) 

SIGYPlNXT )=STDEVX ( 5) 

SIGZP(NXT )=STCEVX(6) 

SIGALP! NXT )  =  STDEVX ( 7  ) 
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C  CONVERT  ESTIMATED  STATE  TO  RADAR  POLAR  COORDINATES 

C  STCRE  ESTIMATED  STATE  VECTOR  IN  OUTPUT  ARRAYS 

C 

5793  CALL  X YT CR ( E S T AT E ( 1 ), ESTATE ( 2 ), E STATE ( 3 ) y E STATE ( 4 ) t E STATE ( 5 ) , ESTAT 
1E(6),RHAT(NXT),AHAT(NXT  )  ?  EHAT ( NX  T ) , RPHA  T ( NX  T )  ,APHAT I  NX  T ) , EPHAT  t  NXT 
2)  ) 

XHAT ( NXT  )  =  E  S  T  A  T  E  (  1) 

Y  HA  T ( NXT )  =  ES  T  AT  E ( 2  ) 

ZH AT ( NX T  )  =  EST ATE ( 3  ) 

X  PH  AT (NXT  )=ESIATE(4  ) 

YPHAT (NXT )=ESTATE( 5) 

Z  PHAT (NXT  )  =  ESTATE( 6) 

ALPHA( NXT )  =  E  S  T  A  T  E  ( 7) 

BETAH(NXT)=1. /ESTATE! 7) 

HNXT=CSCRT( ESTATE!  i  )**2+ESTATE(2 )  **  2+ ( ESTATE ( 3  )  +  RE ) **2 ) -RE 
EE  IGHT (NXT  )=HNXT 
IF!PRNT)581, 579.579 
C 

C  PRINT  CUT  ESTIMATEC  STATE 

C 

579  h R I T E ( 6 . 580  ) 

580  FORMAT (/5X. • ESTIMATED  POINT'/) 
^KITE(6»50)TlPE(NXT)?REAT(NXT)tAHAT(NXT),EHAT(NXT)»RPHAT(NXT) ,  APHA 

lT(NXT),tPHAT(NXT),ESTATt(l),ESTATE(2),ESTATE(3),ESTATE(4),ESTATE(5 

2),ESTATE(6),HNXT,BETAF(NXT) 

50  FORMAT ( 5X  » 1 T  IMfc  ='yF10.4/5Xy'RA  ^ • , F 1 5. 2 » 3X  .  • A Z  =  • f F  10. 6 , 3X . • EL  *• 
1  »F10*6»JX, • R  A  P  = • fFIC. 2»3X»  '  A  Z  P  =  ' |FlC.6y3X»  *  E  L  P  =  ' .F1C.6/5X. • X  =  ’  ,F 
210.2»3Xt • Y  =• .F10.2, 3X. 'Z  =  • , F  1 0 . 2 , 3 X , ' X P  = ' , F 10 . 2 . 3 X , • YP  =',F10 
3.2, 3X  ,  •  ZP  =' »F10.2/5Xf 'HEIGHT  =',5XtF16.2y2CXy'BETA='y5X,F10.2//) 

C 

C  COMPUTE  DIFFERENCES  BETWEEN  ESTIMATE  AND  MEASURED  DATA  POINT 

C 

581  CRINXT  )=RFAT(NXT)-RGN(NXT) 

CA (NXT  )  =  AH  AT (NXT )-AZN( NXT  ) 

DE (NXT  )  =  EH  A  T (NXT  )  -CLN ( NX  T  ) 

DRP( NXT )=RPhAT (NXT  )-RAPN( NXT ) 

25  IF(NN-NCATA)27,26,26 

26  KMAX=INGEX 
CC  TC  AO 

27  IF !  INCCX- IMAX  >  1CC0. 28.28 

28  KMAX=IMAX 
C 

C  WRITE  OUTPUT 

C 

AO  V*R  I  TC  (6  t  10  )  (  LABEL  (  1  )  ,  1=  1  ,  18  ) 

A  20  kR  I  TE ( 6 . 4  3  ) 

A3  FCRMAT(5X.'MEASUREMENT  D A T A ' / / 6X ,  ' T I  ME ' , R X  , 'RANGE  » , 7X  , 'RADCT •  , 17X , 
l'AZIM' v  2CX  * •ELEV1//) 

fcRI TE (0.44  )  ! T  IME( KK  )  ,RGN(KK  )  ,RAPN( KK ) , AZN(KK ) ,ELN(KK)  .KK=1 «  KMA  X ) 

AA  FCRMAT(3X.F1C.3|2X,F1C.2,2X,F1C.2,14X,F10.6,14X.F10.6) 

WRI  TE  (6>t  10)  (  LABEL  (  I  ),  1  =  1.  18) 
h  R I T  E ( 6 . 60 ) 

60  FORMAT (  5X , ' C S T I M A T E D  VALUES'//) 

V*R  I  TE ( 6 . 4  1  ) 

A  1  FORMAT (  3X, ' TIME', 8X, 'RANG E',7X,  'RACOT 'yTX.'AZIM'.eX.’AZDOT'.TX 

1  v 1 ELEV1  «8X«  *  ELCCT ’ »  7X  » 'HEIGHT  '  ,  6X , • BE TA ' /  /  ) 
^RITE(6,62)(TIME(J),RHAT(J) , RPHA  T ( J  )  ,  A  HA  T  (  J  )  ,  A  PH  A  T  (  J  )  , EHAT ( J )  , E  PHA 
IT ( J ) ,HE IGHT ( J  )  ,BET AH! J ) ,  J=  1 ,  KMAX ) 

62  FCRMAT(3X,F10.3,2X,F10.2, lX,F10.2,2XtF10.6,2X,F10.6,2X,F10.6,2X,Fl 
10.6,2X,F10.2,2X,F 1 C  •  2 ) 
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WRITE(6, 10) (LABEL ( I)«IS1«18) 

WRITE(6,60) 

WRITE(6,413)  ‘ 

WRITE(6,414)(T I  ME  C J),XHAT( J), YHAT( J) ,ZHAT( J)  ,XPHAT( J)  ,YPHAT( J)  ,ZPH 
1AT ( J)  , BETAH( J ) , ALPHAt  J ) , J= 1 ,KMAX ) 

4  14  FCRMAT(3X,F10.3,2X,F10.2,2X, F 10.2 ,  2X,F  10.2 ,  2X,F 10.2 ,2X,F 10.2, 2X, FI 
lC.2,2XfF10.2t2X,F12.8) 

WRITE(6t 10) ( LABEL ( I )» 1=1, 18) 

WRITE(6,63) 

63  FCRMAT  (5X,  ’RESICUALS=(  ESTIMATED  V  ALUE  S  )  -  (  NO  I  SY  DATA)*//) 

W  R  I  T  E ( 6  »  64  ) 

64  FCRMAT (  6  X , ' T I  M  E '  »  8  X , ' RANGE f ,7X,  'RADOT  * ,17X,'AZIM',2CX,'ELEV'//) 
WRITE (6, 44) (TIME( J),DR( J ) , ORP ( J ) » OA ( J) ,  DE ( J ) , J  =  L  , KMAX ) 

900  WRITE(6, 10) ( LABEL (  I ) , 1  =  1,  18  ) 

W  R I T  E ( 6 , 90  1  ) 

901  FORMAT ( 5X  , 1  EXPECTED  RMS  ERRORS'//) 

WRITE< 6,61 ) 

61  FORMAT (  3X  , 'TIME' ,  8X  ,  'RANGE  ' , 7X , ♦ R ADOT ’ , 7X , ’ AZ I M ' , 8X , ' A  ZDCT *  ,7X,  'EL 
1EV'  ,  8  X  »  'ELCCT'  ,7X,  '8ETA' ,  8X ,  'ALPHA'/  /  ) 

WRITE (6,902) ( T I ME ( J ) , S I GR ( J ) , S I  GRP ( J ) ,SIGA(J),S1GAP(J),SIGE(J),S1G 
1EP(J),SIGALP(J)  , J=L,KMAX) 

902  FCRMAT(3XtF10.3,2X,F10.3,2X,F10.2,2X,F10.6,2X,F10.6,2X,Fl0.6,2X,Fl 
1 C  *6 , 14  X  ,  F  1 2  •  8  ) 

WRITE (6» 10) (LABEL ( I  ), 1  =  1, 18) 

W  R I T  E ( 6  »  90  1  ) 

WRITE(6»413) 

413  FCRMAT  (  3X,  *T  IMC,  8X,  'X  ’  ,  11X,  fY't  11X,  •  Z  •  ,  1 IX,  * XOOT'  ,8X,  » YDOT '  ,8X, *  Z 
1 CC T',8X,»B ETA’, 8X,  'ALPHA'//) 

WRITE(6,903)(riME(J),SIGX(J),SIGY(J),SlGZ(J),SIGXP(J),SIGYP(J),SlG 
1 Z  P ( J ) , S I  GAL  P ( J ) , J  =  L,KMAX) 

9C3  FCRMAT(3X,F10.3,2X,F10.2,2X,F10.2,2X,F10.2,2X,F10.2,2X,F10.2,2X,F1 

1C. 2, 14X,F12.8  ) 

L  =  1 

IF (LAST ) 9000 , 1C0C,9C00 

1000  CONTINUE 

1001  GC  TC  1 

8  C  CO  I  F ( NX  T- 1  )900C, 9000,8001 
8C0  1  KMAX=NXT-1 
L  A  S  T=  1 
GC  TC  40 
90C0  RETURN 
ENC 


55 


TABLE  B-III 
SUBROUTINE  TRAJGX 


SUBROUTINE  TRA JGX ( XI , YI , Z I , XDOT 1 , YDOT 1 , ZDOT I , BETA I , TAU  ,TSTEP,ERRM 
IAX  , X  2 , Y2  »  Z  2  »  X DOT  2 ,  Y  DOT  2  9 ZDOT  2,8ETA2*PHIMAT ) 

IMPLICIT  RE AL*8 ( A-H»0-Z ) 

REAL*8  T AU »  TSTEP 

REAL *8  J,K,L,M,N,X1,Y1,Z1,XD0T1,YD0T1,ZD0T1,8ETA1,TINT,TINCR,ERRMA 
1X,X2,Y2,Z2,XD0T2, YDOT2,ZDOT2,8ETA2,PHIMAT 
COMMON  /FCOM/CCORD,DLAT, PRNT/ I COM/KLAMP , MDATA , NN 

DIMENSION  XVCTR(7f 3 ) , 8VCTR ( 7, 3 ) ,XI ( 7* 3 ) ,ERR C 7, 3 ) , AM TR X ( 7 , 7 , 3 ) ,ASQ( 
17,7,3) ,A8< 7, 3), PHI (7,7,3) ,UNI T ( 7, 7 ) , ERRMAG ( 7) , ERRMA  X ( 7 ) ,PHIMAT(7,7 
2) 

C 

C  CHECK  THAT  DATA  I NTER VAL , T AU ,  AND  INTEGRATION  STEP, TSTEP,  HAVE 

C  THE  SAME  SIGN 

C 

TPROD=TAU*TSTEP 
IF ( TPROD) 1,2,2 

1  TINCR*-TSTEP 
MR  I T E ( 6 ,  11  ) 

11  FORMAT (/5X, 'DATA  I NTER VAL , TAU ,  AND  INTEGRATION  STEP, TSTEP,  HAD  OP- 
1POSITE  SIGN. '  /5X  , 'LATTER  SIGN  WAS  CHANGED  TO  FORCE  AGREEMENT'//) 

GO  TO  b 

2  T I NCR  =  T STEP 
5  TM*DA8S ( TAU ) 

TSTM  =  DA8S( TSTEP  ) 

IF(TM-TSTM)3,4,4 

3  T I NCR=TAU 
A  T I NT=TAU 

C 

C  CHECK  FUR  1ST  DATA  POINT 

C 

I F ( NN  -1  )  IOO, 100,89 
C 

C  SET  UP  CONSTANTS  AND  ARRAYS 

C 

100  P I =3 • 14 15926b 
RTDEG= 1 • 8D2/P I 
CMFGA  =  7. 27C—  5 
RLAT=DLAT/RTDEG 
OMC=OMEGA  *CCOS ( RLA  T ) 

CMS=OMEGA*DSIN(RLAT ) 

IF (COORD) 1000, 1001,1001 

1000  RE*2.O925730D7/3. 2008333 

GM= 1.407634000 16/ ( 3.2008333) »*3 
GAMMA* 1.D0/7.D3 
GO  TO  1002 

1001  RE*  2. 092  5738  D7 
GM=  1 . 40 76348 0D 16 
GAMMA*. 3O40DO/7.D3 

1002  DO  101  J J* 1 , 7 
DO  101  KK* 1,7 

101  LN I T ( J J , KK ) =0. 

CO  102  J J= 1 , 7 
ERR ( J J , 1 ) =0 • 

ERR ( J J , 2 ) =0. 

ERR ( J J , 3  )  =0. 

102  UN IT ( J J , J J )=1. 

89  IF(TAU) 103,90, 103 
C 

C  CATA  INTERVAL, TAU,  =  G. 

C  SET  OUTPUT  STATE  =  INPUT  STATE,  TRANSITION  MATR I X* I  DENT  I T Y  MATRIX 

C 

90  X 2=  X 1 
Y2  =  Y  1 
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12  =  1 1 

X  00T  2  =  X  DOT  1 
Y0CT2  =  YDO  T  1 
Z  OCT  2=ZOOT 1 
BET  A2=BETA  1 
CO  91  J J= 1 »  7 
00  91  KK  =  1*7 

91  PHlMATt JJ,KK)=UNIT( JJ,KK) 

RETURN 

C 

C  SET  INDICES 

C 

103  NPO I NT= 1 
KOUNT  = 1 
CT  =  T  INCR 
NEXT=2 
NL AST=3 
T  =  0. 

K0ELT=0 
PHSK I P=0 • 

T  SC  =  T I NC  R**2 
C 

C  SET  UP  INITIAL  STATE  VECTOR  ANO  TRANSITION  MATRIX 

C 

X VCTR ( 1,  1)=X1 
XVCTR ( 2 , 1 ) =Y  1 
X VCTR ( 3  *  1 ) =Z 1 
XVCTR ( 4 , 1 )  =  XCOT 1 
XVCTR ( 5 , 1 )=Y00T1 
XVCTR( 6 , 1 I-ZOOTl 
XVCTR  (  7,  1)=1./BETA1 
CO  104  J J*  1 , 7 
CO  104  KK= 1 1  7 

104  PHI  (JJfKK* 1 )  =UN I T ( J  J  t  KK ) 

300  CONTINUE 

C 

C  CALCULATE  ORAG  VELOCITY  AND  HEIGHT 

C 

VO=OSQRT (XVCTR(4,NP0INT  )  **2  +  X VCTR ( 5 , NPC I  NT ) **2  +  XVCTR ( 6 , NPO I  NT )  **2) 
R=DSORT ( XVCTR (  1 , NPO I N T ) **2  + XVCTR ( 2 t NPO I N T ) **2+( XVCT R ( 3 , NPO I  NT ) +  RE ) 
1**2) 

HT  =  R-RE 

IF(HT)3001, 3003, 3003 
C 

C  NEGATIVE  HEIGHT,  APPARENT  EARTH  IMPACT,  RETURN  TO  CALLING  PROGRAM 

C 

3001  hRITE(6,3002)HT,T, ( XVCTR ( J J ,NPOI NT ) , J J= 1 , 7 ) 

3002  FORMAT (  /  5X , •HEIGHT*1 ,F 10*2, •  , AFTER  '«F10.5«*  SEC.  OF  INTEGRATION 
1 —  APPARENT  EARTH  IMPACT  •  /5X  ,•  ST  ATE  VECTOR*  •  /  70 1 7 . 0  /  5X, 'RETURNED 
2 TO  CALLING  PROGRAM*///) 

N  EXT  =  NPO I  NT 
GO  TO  600 

3003  FKMFT=HT /1.03 
RHC=0. 

CALL  A  TM ( HKM FT , RHO ) 

ALPHA  =  XVCTR(7,NPCINT  ) 

C 

C  SET  UP  EQUATIONS  OF  MOTION 

C 

A=— RH0*V0/2 • DO 
W*GM/R**3 
8*0MEGA**2-W 
C*2 . D0*0MS 
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E*-2.D0*0MC 

G=0MS**2-W 

J=-OMS*OMC 

K=J*RE 

N=CMC**2-W 

C=N*RE 

XPP=A*XVCTR(7,NP0INT)*XVCTR<4,NP0INT)+B*XVCTR< 1 , NPO I  NT > +C *XVCT R < 5 , 
1NP0INT )+E*XVCTR<6,NP0INT) 

YPP=-C*XVCTR(4,NP0INT  ) +G*X VCTR < 2 , NPO I  NT ) ♦ A* XVCTR < 7 , NPO I  NT  )*XVCTR{5 
1 ,NPOINT )+J*XVCTR<3,NPOINT  )+K 

ZPP=-E*XVCTRU,NPOINT)  +  J*XVCTR<  2 , NPO I  NT ) +N+XVCTR < 3 , NPOINT ) + A  +  XVCTR 
1 { 7  , NPO I  NT )*XVCTR(6,NP0INT  )  +  Q 
C 

C  FILL  IN  STATE  VECTOR  MATRICES 

C 

B VCTR { 1 , NPO I  NT )  =  XVC  TR ( 4»N POINT) 

BVCTR{ 2, NPO I  NT )  =  XVCTR < 5 , NPO I  NT ) 

BVCTR { 3 , NPO I  NT  )  =  XVCTR < 6 , NPO I  NT  ) 

BVCTR(4,NP0INT )=XPP 
BVCTR { 5  *  NPO I  NT  )  =  YPP 
BVCTR(6,NP0INT)=ZPP 
BVCTR(7,NP0INT)=0. 

C 

C  CALCULATE  SYSTEM  MATRIX  A 

C 

CO  301  J J=  1 , 3 
DO  301  KK=  1*7 

301  AMTKX( JJ,KK, NPCINT )=0. 

AMTRX ( 1, 4, NPCINT  )=1. 

AMTRX (2, 5, NPCINT  )  =  1 • 

AMTRX(3»6»N POINT )  =  1  • 

C 

C  CALCULATE  DERIVATIVES  OF  COEFFICIENTS 

C 

DA=~A*GAMMA/R 

CACX  =  DA*XVCTR<  1  , NPO  I  NT  ) 

LAOY  =  DA*XVCTR( 2, NPO I  NT ) 

DADZ=DA*(XVCTR(  3,NPGINT)+RE  ) 

IF ( VD ) 70 1 y 702,701 

701  DAP=A/VD**2 
GO  TO  703 

702  DAP=A/1.D~4 

703  CONTINUE 

CADXP=CAP*XVCTR( 4,NP0INT ) 

DADYP  =  0AP*XVCTR(5,NP0INT  ) 

CADZP=DAP*XVCTR(6,NP0INT ) 

DB=-3.D0*W/R**3 

C8CX  =  L)B*XVCTR(  1,  NPO  I  NT) 

D8DY=DB*XVCTR( 2, NPO I NT ) 

DBDZ=DB*( XVCTRt  3, NPO I  NT )+RL ) 

CG  DX  =  DBDX 
CGCY=DBDY 
CGDZ=DBOZ 
CNDX  =  DBDX 
CN  CY  =  DBDY 
DN  DZ  =  D8DZ 
CQDX=RE*DBDX 
GQDY=RE*DBDY 
DCCZ=RE*DBDZ 
C 

C  FOR  CONSTANT  BETA 

C 

CALFDX=0. 
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DALFDY=0 • 

calfdz=o. 


OALAOX=ALPHA*OADX+A*DALFOX 

DALAOY=ALPHA*OAOY+A*OALFOY 

DALADZ=ALPHA*OAOZ+A*DALFOZ 

AMTRXt 4,  l,NPOINT)=B+XVCTR( 1 , NPO I  NT ) *DBDX  +  XVCTR ( 4 , NPO I N T ) *DAL ADX 
AMTRXt 4,2#NPGINT)=XVCTR( 1 , NPO I NT ) *DBDY+XVCTR ( 4 t NPOI NT ) *OALADY 
AMTRX(4, 3,NPClNT)=XVCTRt 1 , NPO I  NT ) *DBDZ  +  XVC TR ( 4 , NPO I  NT )*OALAOZ 
AMTRXt A, A, NPO I  NT )=ALPHA*( A  +  XVC TR < 4 , NPO I  NT )*DADXP) 

AMTRXt  4,5,NPOINT )  =  XVCTR ( 4 , NPO I N T ) * AL PHA*D ADYP  +  C 
AMTRXt 4, 6, NPGINT )=XVCTR( A, NPO I  NT ) * ALPHA*OAOZP+E 
AMTRX(4,7,NP0INT)=A*XVCTR(4,NP0INT) 

AMTRXt 5,  1,NP0INT)=XVCTR(2,NP0INT)*0G0X+XVCTR(5,NP0INT)*0ALADX 
AMTRXt  5, 2 t NPO I  NT ) =G+XVCTR t 2 , NPO I  NT ) *OGOY+ XVCTR ( 5 , N PO I  NT ) *OAL AOY 
AMTRXt  5,3,NPOlNT)=J+XVCTRt 2,NPOINT)*DGDZ+XVCTR( 5, NPOI NT  ) *DAL ADZ 
AMTRXt  5, A, NPO I  NT )= XVCTR t  5, NPO INT ) * ALPHA*DA DXP- C 
AMTRXt  5, 5, NPO I  NT )  =  ALPHA*( A+XVCTR ( 5, NPO INT ) *OADYP ) 

AMTRXt  5,6,NP0INT)=XVCTR{ 5, NPO I NT ) * AL PHA*DADZP 
AMTRXt  5,  7,  NPO  I  NT  ) = A*XVCTR t 5 ,NPO INT ) 

AMTRXt 6, 1,NP0INT)=DQDX+XVCTR( 3,NP0I NT ) *DNDX+XVCTR ( 6 ,NPO INT ) *OALAOX 
AMTRX(6,2,NP0INT)=J+DQ0Y+XVCTR{ 3,NPO INT )*DNDY+XVCTR ( 6, NPOI NT )*DALA 
ICY 

AMTRX(6,3,NP0INT ) =N+X VCTR ( 3 ,NPO I  NT ) *DNDZ  +  OQOZ+XVCTR ( 6 , NPOI NT ) *OAL A 

1CZ 

AMTRXt 6,4, NPO I  NT )  =  X VC TR < 6 , NPO I NT ) * AL PHA*OAD XP-E 
AMTRX(6,5,NPQINT)=XVCTR(6,NP0INT)*ALPHA*0A0YP 
AMTRXt 6, 6, NPO I  NT ) = AL PH A* t  A+X VCTR ( 6 , N PO I NT ) *OAOZP ) 

AMTRXt 6, 7, NPOI NT ) =A*XVCTR ( 6, NPO I  NT ) 

AMTRXt  7, I , NPGINT )  =  OAL FOX 
AMTRX(7,2,NPGINT)=OALFDY 
AMTRXt 7 ,3, NPO I  NT )=OALFOZ 
CO  3010  J  J  =  4 , 7 
3010  AMTRXt  7,JJ,NP0INT )-0» 

LO  302  J J  =  1 , 7 
ABt JJ, NPGINT )=0. 

CO  302  LL=  1 , 7 

302  ABt JJ,NP0INT)=AB( J J , NPO I  NT ) +AMT RX < J J , L L , NPO I NT ) *BVC TR ( LL , NPO I  NT ) 

IF (PHSKIP)400,303,400 


C 

C  CALCULATE  TRANSITION  MATRIX 

C 

303  CO  304  J J=  1 , 7 
CO  304  KK  = 1,7 

ASCt JJ,KK, NPCINT )=0. 

CO  304  L L=  1 , 7 

304  ASC ( J J,KK, NPCINT ) = ASQ ( J J , KK , N PO I NT ) +AM TRX <  J J , LL , NPO I N T ) * AM TRX ( LL , 
IKK, NPGINT) 

kpcint=nlast 

CO  305  J  J  =  1 , 7 
CO  305  KK= 1 , 7 

305  PHI ( JJ, KK,KPCINT )=UNIT( JJ,KK)  +  TINCR*AMTRX( JJ,KK,NPOINT )+TSQ*ASQ(  JJ 
l»KK,NP0lNT)/2.00 

CO  306  J J=  1 , 7 
CO  306  KK= 1,7 
PHI t JJ,KK,NEXT )  =  0, 

CO  306  LL  =  1 , 7 

306  PHI t  JJ,KK,NEXT )-PHI ( J J , KK , NEXT ) +PH I ( J J , LL , KPO I  NT ) *PHI ( LL , KK , NPO I N T 

1  ) 

C 

C  INTEGRATE  OVER  INTERVAL  T I NCR 

C 

IF (KOELT*KCUNT ) 320, 320,330 
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320  CO  321  J J  =  1 1  7 

321  XI ( JJ,NEXT)=XVCTR( J J , NPO I NT ) +T I NCR*BVCTR ( J J , NPO I  NT ) +TSQ*AB ( JJ, NPOI 
1ND/2.00 

GO  TO  350 

330  DO  331  J J- 1 1  7 

331  XI ( JJ, NEXT) *XVCTR(JJt NLAST ) *2. 00*T I NCR*B VCTR ( JJ,NLAST) +2.00/3. DO*T 
1SQ*(  ABUJt  NLAST)+2.D0*AB<  JJ,NPOINT)  ) 

350  NPT=NPO I  NT 
NPC I  NT  =  NEXT 
CO  351  J J  =  1 1 7 

351  XVCTRl JJ,NPOINT )=Xl < JJ ,NEXT) 

PH  SK I P- 1 • 

GO  TO  300 

400  CO  4000  J  J=  1 »  7 

XVCTR(JJfNEXT)-XVCTR(JJ»NPT  )  •*■•500*  T I  NCR*  (BVCTR(JJfNPT)+BVCTR(JJ,NP 
101  NT) )+TSQ*( AB(JJ,NPT)-AB< JJ,NEXT) )/ 1*201 
ERR(JJtNEXT)*XI ( J J , NEXT )  — XVCTR ( J J  «  NEXT ) 

ERRMAGI JJ)  =  0ABS( ERR ( JJ, NEXT)  ) 

4000  CONTINUE 
C 

C  TEST  INTEGRATION  CONVERGENCE  ERRORS 

C 

CO  401  J J= 1 »  7 

IF ( ERRM AG ( J J  )  -ERRM AX ( J J )  ) 401, 500,500 

401  CONTINUE 

T  =  T  +  T  I  NCR 
TREM=T  AU-T 

IF( TREM )4010f600|4010 
4010  TREMAG=UABS< TREM) 

IF ( TREMAG-TSTM  )4C5,402,4C2 
405  T I NCR=T  REM 
TSC=TINCR**2 
K0LNT=0 
C 

C  L  PC  AT  E  SUBSCRIPTS 

C 

402  NEXT=MLD(NEXT,3)+1 
NL AST-NPT 

KDELT  =  KDEL  T+  1 
PHSK I P=0 • 

GO  TO  300 
C 

C  CONVERGENCE  ERRORS  TOO  LARGE  —  PERFORM  ITERATION 

C 

500  CO  501  KK=  1 , 7 

501  XI ( KK, NEXT )=XVCTR(KK,N EXT) 

PHSKIP=1. 

GO  TO  300 
C 

C  SET  UP  OUTPUT  STATE  VECTOR  AND  TRANSITION  MATRIX 

C 

600  X2  =  X VC T R ( 1 , NEXT  ) 

Y2=XVCTR(2,NEXT ) 

Z2  =  XVCTR<  3, NEXT ) 

XDCT2  =  XVCTR( 4,NEXT  ) 

YCCT2=XVCTR( 5, NEXT  ) 

ZCCT2=XVCTK(6,NEXT  ) 

BETA2=i./XVCTR(7,NEXT) 

CO  601  J J=  1 , 7 

CO  601  KK  =  1 , 7 

601  PH IH AT ( J J, KK  )=PH I ( JJ,KK,NEXT  ) 

RETURN 

ENC 
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TABLE  B-IV 

SUBROUTINE  ESTMAT  (VERSION  I) 


SUBROUTINE  ESTMAT ( STATEM, STATEP, PH IMTX, ESTATE ,DEVX, DEVR ) 

C 

C  SUBROUTINE  ESTMAT  USING  EQUATIONS  IN  MCfcERY  ARTICLE 

C 

IMPLICIT  REAL*8( A-H,C-Z) 

PE AL*8  $TATEM, STATEP, PHI MTX, ESTATE 

COMMON  /ACCM/CCVARI 7 ) , S I GMA ( 7 ) /FCOM/COCRD , DLAT , PRN T / I C CM/KL AMP ,MDA 
I T  A , NN 

Cl  MENS  I  ON  PHIMTX(7,7),CUMMY(7,7)  ,  M(7«7)V0X(7) , STATEP (7) 

Cl  MENS  I  ON  ST  AT  EM ( 7), ESTATE! 7) ,  H  t  7 , 7  ) ,R( 7, 7)  ,S( 7,7,2  ) 

C  I  ME  NS  ItJN  C  E  VX  (  7  )  , CEVR (  7  ) 

Cl  MENS  I  ON  CU77(7,7),SR(7,7,2),PHIT(7,7), UN  I T ( 7 , 7 ) 

Cl  MENS  ION  HT (7,7 ) , RST ATE ( 7  )  ,MV I ( 7) ,MV2(7) , ARRAY (49) 

NZ  ERC  =  0 

CCCRDM=DABS(COCRC  ) 

C 

C  TEST  FOR  1ST  TIME  THROUGH 

C 

IF(NN-l)  I,  I,  100 
C 

C  1ST  TIME  THROUGH  -  INITIALIZE  ARRAYS  AND  MATRICES 

C  SET  UP  APR  I C  R I  COVARIANCE  MATRIX 

C 

1  NS  T  ART-  1 
IF(MDATA)2,2C0,3 

2  M  D=-MD AT  A 
GO  TO  4 

3  MC  =  MD AT  A 

4  M P 1 =ML+  1 
LMAX=MC **2 
NK=  1 

NKM  1  =  2 
CC  7  J-  1 , 7 
CC  7  K=  1 1  7 
LNIT ( J,K ) =  C • 

M  J , K ) =0 . 

CX( J)=0. 

H ( J , K ) -0 • 

5  R  (  J,K  )  =  0 • 

S  R ( J , K , 1 ) =0 • 

7  S( J,K, 1  )  =  0  . 

CC  6  J= 1 , MC 
H  (  J  ,  J  )  -  1 . 

6  R( J,  J)  =  SIGMA( J )**2 
88  CC  8  J=  1  ,  7 

8  LNIT ( J , J  )  =  1 . 

I F (CCCROM- 1 . )  9C , 90 , 80 

90  CC  9  1  J  =  1 , 7 

91  S( J, J, 1 )=CCVAR< J)**2 
GO  TO  1302 

80  CC  81  J  = 1 ,  7 

81  SR( J, J, 1 )=CCVAR( J )**2 
IF ( PR N  T ) 10,9,9 

9  kR I T  E ( 6 ,  1 1  HO ) ( (SR(J,K,NK),K=1,7),J  =  1,7) 

1180  FORMAT (  /  6  X  ,  *  A  P  P  I C  P  l  COVARIANCE  MATRIX  IN  RADAR  COORD  I N A T E S • / ( 7 D 1 7 • 
18  )  ) 

10  CC  1181  J  = 1 , 7 
CC  1181  K  =  1 , 7 

1181  CU77 (J,K  )  =  SR(J,K,NK ) 

CALL  RCCVTX(CU77, ESTATE, CUMMY) 
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CC  1182  J=  1 »  7 

CC  1182  K=  1 »  7 

1182 

S<  J,K,ISKI=CUMMY<  J,K) 

r 

GC  TC  1302 

L. 

c 

r 

LPCATE  1NCICES  NK.NKMl 

L. 

100 

IF (NK-1 )  1,  102, 103 

102 

NK  =  2 

NKM1=1 

GC  TC  104 

103 

N  K  =  1 

r 

CM 

II 

L. 

c 

r 

COMPUTE  CCVAR1ANCE  MATRIX  S(K,K-1)  IN 

XYZ  CCCRDINATES 

L 

104 

CC  105  J=  1 ,  7 

CC  105  K=  1  ,  7 

PhIT (J,K)=PHIMTX(K,J) 

105 

CUMMYt  J  ,K )  =  S  (  J  *  K  , NKM  1  ) 

CALL  CMTMULt  CUKMY, PHIT ,0077,7,7,7) 
CALL  CMTMUL ( PF  IMT  X  ,  CU  7  7  ,  CUM  MY  ,  7 ,  7,7) 
NSTART=C 

CC  106  J=  1  ,  7 

CC  106  K=l,7 

106 

S( J,K,NKM1)=CUMMY( J,K) 

c 

IF  (  PPM  )  1060, 1 C6C  ,  1 C  5  8 

c 

PRINT  CUT  TRANSITION  MATRIX 

c 

r 

PRINT  CUT  COVARIANCE  MATRIX  S ( K , K —  1 ) 

OF  PREDICTED  STATE 

L 

1058 

RRI TF (6 ,30C  )  (  ( PH  I M TX ( J , K ) , K=  1 , 7  )  , J  =  1  , 

7) 

300 

FCKMAT (/5X,  • TRANSITION  MATRIX  USED  IN 
R  R I T  E ( 6 , 1059) 

ESTIMATION 

’/( 7017.8) ) 

1059 

FCRMA1  ( / 5 X ,  'COVARIANCE  MATRICES  S(K,K 

-1  )  ■  ) 

r 

Vs  R  1  T  E  (6, 1183)  (  (S(J,K,NKM1  )  ,  K  =  1 , 7  )  ,  J  = 

1  ,7) 

0 

c 

r 

CALCULATE  FYERIC  COVARIANCE  MATRIX  OF 

PREDICTED 

STATE 

L 

CALL  XCCVTR ( LUMMY , ST  A T EP , DU77 , 7  ) 

CC  2016  J=  1 ,  / 

CC  2015  K=  1  »  7 

2015 

F( J,K)=CU77( J,K)/CSCRT( UL77( J , J ) *DU 7 7 ( K , K  )  ) 

r 

2016 

H( J,J)=DSCRT(LL7/( J  ,  J  )  ) 

L 

c 

r 

PRINT  CUT  FYRRIC  COVARIANCE  MATRIX  OF 

PREDICTED 

STATE 

L 

r 

RRITF(6»13C1)(  ( H ( J , K  )  ,K=1,7),J  =  1,7) 

L 

c 

r 

COMPUTE  inE  I GFT  ING  MATRIX  Vs 

L 

1060 

CALL  XCCVTRt  CUMMY ,STATLP,H,-1  ) 

CC  1C  (  J=1  ,  7 

CC  107  K  =  1  »  7 

107 

FT (J,KJ=h(K,JJ 

CALL  CMTMULt  CUMMY, FT ,DU77,7,7,MD) 

CALL  CMMULt  F  ,  CU7  7  ,  CUMM  Y  ,  ML  ,  7  ,  MD  ) 

CC  108  J  = 1 , M  C 

CC  ICR  K= 1 , M  C 

108 

EU77(J,K)=CUMMY(J,K)+R(J,K) 
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CC  109  J=MP 1*7 

CC  109  K=PP1,7 

109 

CU77 1 J, K ) =0. 

CC  1090  J=I,PC 

CC  1090  K=  1 , PC 

L  =  PC  *  1 J-  1  )+K 

1090 

ARRAY  1 L ) =CU77 1 J,K  ) 

CALL  P 1 N V ( ARRAY,MC,CETERM,PV1,MV2> 

CC  1091  L=I,LMAX 

j=(l-i)/pc+i 

K=L-PC* 1 J-  1  ) 

1091 

CU  7  7 ( J , K  )  =  A  HR  A Y ( L ) 

CALL  C NT  NUL ( E T ,  DO 7 7 , DUNM Y , 7 , MD , MD ) 

cc  no  j=i  ,7 

CC  110  K=l,7 

1  10 

CU77( J»K)=S( J,K,NKN1) 

CALL  CNTNULt  CU 7 7 , CUNN Y , W , 7 , 7 , MD ) 

1  F  (  PRM  11102,1  102,22  19 

L 

c 

PRIM  CUT  PARTIAL  CLRIVATIVE  MATRIX  H=CR/DX, 

AND  WEIGHTING  MATRIX 

c 

EVALUATEC  AT  PRECICTED  STATE 

L 

2219 

WRITE (6,2220 ) ( ( h( J,K ) ,K=I , 7) , J=I ,7  ) 

2220 

FCRPAT ( /bX,  ' E  PATRIX  (CR/DX)  EVALUATED  AT  PREDICTED 

STATE  1 / ( 7D  17.8 

1  )  ) 

110  1 

W  R  l  T  F  (  6  , 1111  )  (  ( W ( J  J  »  K  K ) ,  K  K  =  1 , 7  ) ,  J  J  =  I  ,  7  ) 

r 

mi 

FCRNAT (/5X, 'WEIGHTING  MATRIX  ^•/(7C17.P)) 

L 

c 

r 

CALCULATE  COVARIANCE  MATRIX  S(K)  OE  ESTIMATED 

STATE 

L 

1102 

CALL  CNTNUL(W,E,CUMMY,7,ML),7) 

CC  117  J  =  I  »  7 

CC  117  K ” 1 , 7 

117 

DUMMY  (  J,K  )  =UM  T  (  J,  K  ) -DUMMY  (  J,K  ) 

C  0  lib  J  =  I  ,  7 

DC  lib  K  =  l  ,  7 

S  lJ,K,f\K)=C. 

CC  lib  L  =  1  »  7 

l  18 

S  (J,K,NK)=S  (J,K,NK)+CUMMY( J,L)*S  (L,K,NKM1) 
IF  ( M  T  A  1  A  )  1056 , 2CC  ,1112 

10  5  6 

CC  1057  J*  l  , 7 

1  C  5  7 

LSTATEl J)=STATEP( J) 

r 

CC  1C  10C0 

L 

c 

r 

COMPUTE  Nth  ESTIMATE  CF  STATE  VECTOR 

L 

1112 

CALL  XYTCR(S1ATEP(1),STATEP(2),STATEP(3),STATEP(4), 

STATEP ( 5 ) , STATE 

lP(6),kGTATE(I),RSTATF(2),R$TATL(3),RSTATE(4), 

RSTATEI5)  ,RSTATE(6)  ) 

RSTATE ( 7  )  =  S  T  A  T  t  P  (  7  ) 

CC  113  K= I , MC 

113 

CX(K)=STATEM(K)-RSTATE(K) 

IF(PRNT)  1  132,1  132,1  1  30 

L  t  30 

VsRITF(6, 1131)  (CX(J)»J=1»7) 

1131 

TCRMAT(/5X,'Y-E(X)=t,/CI7.b) 

I  132 

CC  114  J=  t  ,  7 

CONN Y ( J ,  I  ) -  0 • 

CC  114  K= I , N  C 

114 

CUNNY( J , 1 ) = CUN NY ( J, l)4^(J,K)*CX(K) 

CC  115  I  ,  7 

1  15 

ESTATE(K  )=STATEP(K)  +CUMMY  (  K  ,  1  ) 
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c 

c 

r 

CALCULATE  HYER.IC  COVARIANCE  MATRIX  OF  ESTIMATEC  STATE 

1000 

CC  1190  J=  1 , 7 

CC  1190  K= 1  *  7 

1190 

CUMMY{ J,K)=S(J,K,NK) 

CALL  XC0VTR1CUMMY, ESTATE, DU77, 7) 

CC  1300  J=  1 , 7 

CC  1297  K= 1 , 7 

IF(CU7  7( J,J )  )1291, 1291, 1290 

1290 

I  F ( CU77 ( K ,K )) 1291,  1291, 129 A 

1291 

h  R I TE ( 6 , 1292) 

1292 

FORMAT {/5X, ' ***NEGATIVE  C I AGONAL  TERMS  CF  COVARIANCE  MATRIX 

IN  RAD 

1AR  PCLAR  CCCRCINATES****/8X, 'DISREGARD  HYBRID  MATRIX  PRINTED 
2'//) 

PRN  T=  1  . 

GO  TC  1399 

BELOW 

1294 

CONTINUE 

CUMMY{J,K)=CU77{J,K)/DSQRT(CU77(J,J)*DL77(K,K) ) 

CCRR=DABS( CUM MY ( J,K ) ) 

IF  (  CCRR-U  00001  )  129  7, 1297, 1298 

1298 

WRITE(6,1299) 

1299 

FORMAT (///5X, • ******  CORRELATION  COEFFICIENT  EXCEEDS  1.  ******•//) 
PRNT=  1  • 

1297 

CCNTINUE 

1300 

c 

CUMMYi J,J )  =  CSCRT { CU771 J, J  )  ) 

V 

c 

r 

STORE  DIAGONAL  ELEMENTS  CF  COVARIANCE  MATRICES 

V 

1399 

CC  1400  J=  l ,  7 

CEVR( J)=CUMMY{ J, J) 

1400 

r 

CEVX{ J)  =  CSQRT(S( J , J , NK  )  ) 

I F { PRNT ) 1302, 1401,  1401 

V 

C 

r 

PRINT  CUT  STATE  ESTIMATE  AND  COVARIANCE  MATRICES 

L 

1401 

h  R I TE ( 6 , 1150)  ( ES T A TE { J  )  , J= 1 , 7 ) 

1150 

FCRMAT(//5X, 'ESTATE  =  ESIMATE  OF  STATE  VECTOR  * /7D 1 7. 8 ) 

1001 

V.R  I  TE  (  6 , 1179) 

1179 

FORMAT (/5X, 'F INAL  ESTIMATE  OF  COVARIANCE  MATRICES*) 

V*RITE<6, 1183  )(  I  S(  J,K,NK  ),K  =  l,7  ),  J*l,7  ) 

1183 

FORMAT ( //5X , ’COVARIANCE  MATRIX  IN  XYZ  COORDINATES* /I  7017*8 ) ) 

13001 

hRITE(6,1301)((  DUMMY  (  J , K ) , K- 1 , 7  ),  J*l,  7) 

1301 

FCRMAT1/5X, 'HYBRID  MATRIX  IN  RADAR  COORD  I NA TES * /5X ,  D I  AGONAL 

TERMS 

r 

1  ARE  STANDARD  DEVIATIONS,  OFF-DIAGONAL  TERMS  ARE  CORRELATION 
2ICIENTS'/( 7D17.8) ) 

CCEFF 

V 

C 

r 

TEST  FOR  MEMORY  CLAMPING 

L 

1302 

IF ( KLAMP  )  1 C  5  5 ,  1055, 119 

1  19 

I FINN-KL AMP) 1055,  1191, 1191 

1191 

DC  1192  J  =  1 »  7 

CC  1192  K  =  L ,  7 

L  =  7*(  J-D+K 

1192 

ARRAYtL)=S(J,K,NK) 

CALL  MINV t ARRAY, 7 , C ET , MV  1 , M V2 ) 

IF (NN- KLAMP) 1055, 1193, 1194 

1193 

CEL AST  =  OET 

C 

GC  TC  1055 
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C  SCALE  COVARIANCE  MATRIX 

C 


1194  SCALE=( DELAST/ DET )**. 142857143 
DO  1195  J=  1 » 7 

CO  1195  K=  1 »  7 

1195  S( J,K,NK)*SCALE*S< J,K,NK) 

1055  IF(NSTART) 100, 1200, 100 
1200  RETURN 

200  V«RIT€  (6,201) 

201  FORMAT! ///' ******  MCATA=0,  THIS  SUBROUTINE  SHOULD  NOT  HAVE  BEEN  CA 
1LLEC  ****»////) 

RETURN 

END 


4 
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TABLE  B-V 

SUBROUTINE  ESTMAT  (VERSION  II) 


SUBROUTINE  EST MAT  < ST AT EM , S TATEP , PH  I MTX , E S TA TE ,DEVX, DEVR) 

C 

C  THIS  VERSION  OF  ESTMAT  INVERTS  7X7  COVARIANCE  MATRICES 

C  IN  THE  COURSE  OF  CALCULATIONS 

C 

IMPLICIT  REAL  *  8  I A-H ,  O-Z  ) 

RE AL *8  STATEM,STATEP» PH IMTX, ESTATE 

COMMCN  / ACCM/CCVAR { 7 ) , SIGMA { 7) /FCOM/CGCRD ,DLAT , PRNT / I CCM/KLAMP  f MDA 
1 T  A  f  NN 

Cl  MENS  I CN  PHlMTXI7,7)fCUMMY(7,7),DUMI7»7)  ,WI7,7),DXI7),STATEPI7) 
DIMENSION  STATEM<7)fESTATEI7),HI7,7),R<7,7)fSI7,7,2),RSTATEI7) 
DIMENSION  RINVI7,7)»SI I  7 , 7  »  2  )  »  H  T  I 7 , 7 ) , HB ACK ( 7 , 7 ) ,HTBACKI7,7) 
DIMENSION  CU77I7, 7) ,MV1 I 7)  ,MV2< 7 ) *  ARRAY ( 49 )  , SR ( 7* 7 , 2 )  t PHI T ( 7 , 7 ) 

Cl  MENS ICN  CEVX(7)fCtVR( 7) 

NZ  E  RC  =  0 

CCCRDM=  D A BS <  CCCRC  ) 

C 

C  TEST  FOR  1ST  TIME  THROUGH 

C 

IF INN-1  )  1  , 1,  10U 
C 

C  1ST  TIME  THROUGH  -  INITIALIZE  ARRAYS  AND  MATRICES 

C  SET  UP  APR  I CR I  CCVARIANCE  MATRIX 

C 

1  NST  ART= 1 
IF(MDATA)2»  5C0 «  3 

2  MD  =  -MD AT  A 
GC  TO  A 

3  MD=MDATA 

4  MP  I  =  MD  +  1 
NK  =  I 

NKM  1  =  2 
CC  7  J  =  1 f  7 
CXI J)=0. 

CC  7  K* 1 1  7 
DUMMY! J,K)=0. 

DU 77 {  J »  K  )  =  0 • 

MJ,K)  =  0. 

HI J , K  )  =  0 • 

H  B  ACK I J , K ) =0  • 

HTBACKI J,K  )  =0 . 

5  RIJ,K)=0. 

SRI  J,K,  1  )  =0 • 

RINVI  J,K)=0. 

SI  I J,K , 1  )  =  0 • 

SI  I J, K, 2  )  =0 • 

7  $< JfK«l)-0. 

CC  6  J=1  ,MC 

RINVIJ,J)=L./SIGMA<J)**2 

6  RIJ»J)=SIGMA!J)**2 
IFICCCRQM— 1.  )11, 11, 61 

61  CC  8  J  =  1  »  7 

CUMMYI J,  J)=1./C0VAR< J )**2 

8  SRI J, J,  1  )=CCVAR< J)**2 

CALL  XCGVTR { LUMMY, ESTATE, H,-l  ) 

CC  9  J=l, 7 
CC  9  K= 1 »  7 

9  HTI  J,K)=HIK, J) 

CALL  DMTMULIHT ,C0MMY,CU77, 7, 7, 7) 

CALL  CMTMULICU77,H,CUMMY,7,7,7) 
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CC  10  J=  l ,  7 
CC  10  K=  1 , 7 

10  SI ( J,K,1)=CUMMY( J,K) 

IF ( PRNT ) lOOOtlOOlt 1001 

1001  VtRITE(6«  1180)  (  ISRIJfKtNK  )«K*1«  7)  9  J>1«7) 

1180  FCRMAT ( //5X, »CCVAR  IANCE  MATRIX  IN  RADAR  COORD  I  NATES » / ( 7D1 7. 8 )  ) 
fcR  ITE( 6,2011 ) (  (DUMMY!  J,K  ),K=l,7)t  J  =  l,  7) 

GC  TC  1000 

11  CC  12  J=  1 , 7 

S( J,J, 1)=CCVAR( J)**2 
SI ( Jf Jf 1 )=1./S( Jf J» 1) 

12  CU7  7 ( J , J  )  =S ( J  *  J ,  1) 

IF(PRNT)  1000,13,  13 

13  CALL  XCCVTR(CU77, ESTATE, CUMMY, 7) 

CC  1 A  J  =  1 , 7 

CC  1 A  K=  1 ,  7 

1A  SR ( J,K, 1 )=CUMMY( J,K) 

ViRITE(6, 1180  )  (  (  SR(  J,K,NK)  ,  K=l,7)  »J-1,7) 
V»RITE(6,2011)((SI(J,K,NK),K=1,7),J  =  1,7) 

GC  TC  1000 

c 

C  UPCATE  INCICES  NK , NKM  1 

C 

100  I F ( N  K —  I  )  1,  102,  103 

102  NK  =  2 
NKM1M 

GC  TC  1 0  A 

103  N  K=  1 
NK  M 1=  2 

1 0  A  NSTART  =  0 

I  F ( PRN  T ) 106, 105,  105 
C 

C  PRINT  CUT  TRANS  I T I CN  MATRIX 

C 

105  l*RITE(6»200)  (  (PHIMTX(J,K),K=1,7),J=1,  7) 

200  FORMAT ( /5X,  • TRANSIT  ICN  MATRIX  USED  IN  ES T I MA T I ON » / ( 7D 1 7. 8 )  ) 

C 

C  COMPUTE  INVERSE  CCVARIANCE  MATRIX  SI(K,K~1)  IN  XYZ  COORDINATES 

C 

106  CC  2001  J=  1 , 7 
LC  2001  K  =  1 , 7 
L=7*( J-ll+K 

2001  ARRAY (L )  =  PFIMTX(J,K  ) 

CALL  MINV( ARRAY, 7 , C E TERM , M V 1 , M V 2 ) 

CC  2002  L  =  1  »  A9 
J= (L-ll/7+1 
K=L-7*( J-l  ) 

2002  PHIMTX ( J,K)  =  ARRAY (L  ) 

L'C  201  J=  1 , 7 

CC  201  K  = 1 , 7 

PHIT (J,K)=PHIMTX(K,J) 

201  CUMMY(J,K)=SI (J,K,NKM1) 

CALL  DMT MUL ( CUMMY , P F I MT X , DU 77 , 7 ,  7 , 7 ) 

CALL  CM T MUL ( PH  I T , CU77 , CUMMY , 7, 7, 7 ) 

2030  CALL  XCOVT«(RINV,STATEP,h,-l) 

CC  203  J= 1 , 7 
CC  203  K= 1 , 7 
203  HT( J,K)=H(K,J) 

CALL  CMTMUL(FT,RINV,to»  7,7,7) 

IF (PRNT ) 20  39 , 2C  39 , 2C  10 
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C 

C  CALCULATE  HYBRID  COVARIANCE  MATRIX  OF  PREDICTED  STATE 

C 

2010  CO  202  J  =  1 »  7 
CG  202  K=  1 , 7 

202  DU77< J,K)=CUMMY( J,K) 

CC  2003  J=  1  *  7 
CO  2003  K=  1*7 
L=7*( J-l  )+K 

2003  ARRAY(L)=DU77(J»K) 

CALL  M I N V ( ARRAY, 7 , C ET ERM , M V 1 , M V2  ) 

CC  2004  L=  1  *  49 
J= (L-l 1/741 
K=L-7*{ J-l) 

2004  CU7 7  ( J,K)=ARRAY(L  ) 

CALL  XCGVTR(CU77,STATEP,HBACK,7) 

CC  2016  J  =  1  *  7 
CC  2015  K=  1 , 7 

2015  HTBACK  (J  ,K  )  =  FB  ACM  J,K ) / DSQRT { HB ACK { J , J  ) *HBACK { K , K )  ) 

2016  HTBACK ( J, J )  =  CSCRT < HBACK ( J , J  )  ) 

C 

C  PRINT  CUT  COVARIANCE  MATRIX  S ( K , K- 1  )  OF  PREDICTED  STATE 

C  ANC  ITS  INVERSE 

C 

C  PRINT  CUT  HYBRID  COVARIANCE  MATRIX  OF  PREDICTED  STATE 

C 

C  PRINT  CUT  PARTIAL  DERIVATIVE  MATRIX  H=DR/DX,  AND  HTKAN  S*C I NV  MATRX 

C  EVALUATEC  AT  PRECICTED  STATE 

C 

WRITE  (  6 ,  1G59  ) 

1059  FORMAT { / 5 X ♦ ’COVARIANCE  MATRICES  S(K,K-1)») 

VnR  I  TE  (  6 , 20  1 1  )  (  C  DUMMY (  J,K  )  ,K=i ,  7)  ,  J=i,  7) 

2011  FORMAT ( /5X,  *  INVERSE  MATRIX  IN  XYZ  COORC I NA T E S ■ / ( 7D 1 7 . 8  )  ) 
WRITE<6,1183) ( (DU77{J»K)#K=l»7)tJ“it7) 

WRITE (6*  1301  )  (  { HT  BACK ( J  *  K  ) ,  K  =  1 , 7), J- 1 , 7  I 
WRITE  I 6» 2220 H  C  H  < J,K),K*1,7), J=l, 7) 

WRITE (6, 2221 H (W( J,K) fK= 1,7) ,J  = 1,7) 

2220  FORMAT ( /5X  ,  »H  MATRIX  (CR/DX)  EVALUATED  AT  PREDICTED  STATE  1 / ( 7D 1 7 • 8 
1)  ) 

2221  FORMAT (  /  5X  ,  *  W  MATRIX  =  HTR AN SP*CR I N V 1 / ( 7D  1  7 • 8 )  ) 

C 

C  CALCULATE  COVARIANCE  MATRIX  S(K)  OF  ESTIMATED  STATE 

C 

2039  CALL  DMTMUL < W , H , CU77, 7 , 7, 7 ) 

CC  204  J=l,7 
CC  204  K=  1 , 7 

CUMMY  (J,K)-=CUMMY  {  J  ,  K  ) +DU77  (  J,K  ) 

204  SI (J,K,NK)=DUMMY(J,K) 

2050  CC  2005  J=i,7 
CC  2005  K=  1 ,  7 
L  =  7* ( J- 1 )+K 

2005  ARRAY(L)-CUMMY (  J ,  K  ) 

CALL  MINV( ARRAY, 7,DET,MVi,MV2) 

CC  2006  L=  1 , 49 
J=(L-l)/7+l 
K=L-7*( J-i ) 

S ( J , K , NK  )  =  ARRAY ( L  ) 

2006  CUMMY  < J , K )= ARRAY < L  ) 

CALL  DMTMUL(CUMMY,W,CUM,7,7,7) 

IF (MCATA) 1056, 500  ,1058 
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1056  CC  1057  J=  1 , 7 

1057  ESTATEi J)=STATEP( J) 

GC  TC  1 1 A 8 

C 

C  COMPUTE  ESTIMATE  OF  STATE  VECTOR 

C 

105  8  CALL  XYTCRiSTATEPI  1  ) , ST AT EP ( 2  )  , S TA TLP { 3  ) , ST A TE P ( A ) , ST A TE P { 5 ) ,  STATE 
1P{6) , RS T  A T E ( 1) ,RSTATE{2)>RSTATE(3),RSTATE{A) ,RSTATE(5) >RSTATE(6) ) 
RSTATE(7)=STATEP( 7) 

CC  113  K=  1 »  M C 

113  CX(K )=STATEP(K )-RSTATE(K ) 

IF {PRNT )  1  132,1  132,  1  130 

1130  WR I TE ( 6 ,  1131MCX{J)»J=L,7) 

1131  FORMAT (5X, ,Y-F(X)=t,7C17*8) 

1132  CC  2061  J  =  1 ,  7 
CU77 ( J , 1 ) =C . 

CC  2061  L=  1 , PC 

2061  CU77 ( J  ,  1  )  =  CU77 { J  » 1 ) * CUP ( J , L ) * DX { L  ) 

CC  208  J=  1 , 7 

208  ESTATE ( J )  =  S T AT E P {J ) +CU 7 7 { J  ,  1  ) 

C 

C  CALCULATE  FY8RIC  COVARIANCE  PATRIX  OF  ESTIPATED  STATE 

C 

11A0  CC  1  80C  J=  1 , 7 
CC  1800  K=l,7 
1800  DUMMY! J , K ) =  S { J,K,NK) 

CALL  XCOVTRI CUPMY, ESTATE, DU7T, 7) 

CC  1300  J=  1 , 7 
CC  1297  K= 1 , 7 

1F(CU77( J, J )  )  1291  ,  1291, 1290 

1290  IF (CU7  7 (K,K ) ) 1291 , 129  1 , 129A 

1291  WRITEI6  ,  1292) 

1292  FCRPAT ( 15X, ’*** NEGATIVE  DIAGONAL  TERPS  OF  COVARIANCE  MATRIX  IN  RAD 
IAR  POLAR  CCCRCINATES***’/8X,  ’DISREGARD  HYBRID  PATRIX  PRINTED  BELOW 
2*//  ) 

P  R  N  T  =  1  . 

GC  TC  1399 
l  29 A  CONTINUE 

CUMMY(J,K)=CU77(J,K) /CSQRT (CU77(J,J)*DU77(K,K)) 

CCRR=UABS ( CUPPY( J  »K  )  ) 

IF ( CORK-  1  .COCO  1  )  1^9?,  12  97,  129  8 

1298  WRITEI6, 1299) 

1299  FCRMAT(///5X,’*****C0RRELATI0N  COEFFICIENT  EXCEEDS  1.  *♦♦**’///) 

P  R  N  T  =  1  . 

1297  CC N T I NU E 

1300  DUMPY ( J, J)=CSCRT(CU77( J, J)  ) 

C 

C  STCRC  LIAGCNAL  ELEMENTS  CF  COVARIANCE  MATRICES 

C 

1399  CC  l  AGO  J=  l  »  7 

CEVR ( J  )  =  DUMP Y ( J , J ) 

1 A 00  CEVX( J)sCSCRT(S( JtJtNK) ) 

IF ( PRN I  )  1302, 1  1A9,  11A9 
C 

C  PRINT  CUT  STATE  ESTIMATE  ANC  COVARIANCE  MATRICES 

C 

1 1A9  WR1 TE (6 , 1 152 ) ( ( CUP ( J,K ) ,K= i , 7 ) , J= 1 , 7 ) 

1152  FCRMAT(/5X, ’WEIGHTING  PATRIX  S(K)*W  "/(7DI7.8)) 

1150  WR1TEI6, 1151)  (  E  S  T  A  T  E  (  J  )  ,  J  =  L  ,  7  ) 

1151  FORMAT ( //5X, ’ESTATE  =  ESIPATE  OF  S1ATE  VECTCR ’ / 7D1 7 • 8 ) 
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W  R  I  T  E  (  6  »  1179) 

1179 

FCRMAT  (/5X,’FIN£L  ESTIMATE  OF  COVARIANCE  MATRICES’) 

WRITE <6, 1183) ( (S( J, K,NK),K= 1,7), J=l,7) 

WRITE (6, 20  11 ) ( (SIC J,K,NK),K  =  1,7) , J=l,7) 

1183 

FGRMAT  ( //5X,  ’CCVARI  ANCE  MATRIX  IN  XYZ  COORD  I NA  TES  •  /  (  7D 1 7.  8  )  ) 
WR I T  E ( 6 »  1184)  CET 

1  184 

FCRMAT I/SX, • C ET ERM I N ANT= » ,  D I 7. 8// ) 

WRITE (6, 1301) ( C DUMMY (J,K),K= 1,7)  ,J  =  I,  7) 

1301 

FCRMAT ( /5X ,  ■ F Y  ER  I  C  MATRIX  IN  RADAR  COORD  I NA TES ’ /5X , ’ C I  AGON AL 

TERMS 

r 

I  ARE  STANDARC  CEVIATIONS,  OFF-DIAGONAL  TERMS  ARE  CORRELATION 
2ICIENTS'/(7D17.8) ) 

COEFF 

U 

C 

r 

TEST  FLR  M EMC RV  CLAMPING 

t 

1302 

IF  (KLAMP  )  1055,  1055, 1  19 

119 

IF  (NN-KLAMP)  10 55 ,  1  19 3 ,  1 1 94 

1193 

r 

CELAST=UET 

GC  TO  1055 

t 

c 

r 

SCALE  CCVARIANCE  MATRIX 

L 

1  194 

SC AL  E= (DELAST/CET)**.  14  28  57143 

CC  1195  J= 1 , 7 

CC  1 195  K  = 1 , 7 

1195 

SI  (  J,K,NK)=SCALE*SI ( J,K,NK ) 

1000 

CCNTINUE 

1055 

IF (NSTART)  100,  1200,  100 

1200 

RETLRN 

500 

WRITE  (6,501) 

501 

FCRMAT (//5X, 1  *  *  *  M  C  AT  A  =C,  THIS  SUBROUTINE  SHOULD  NCT  HAVE  BEEN  CA 
1LLEU  ***»///) 

RETURN 

ENC 
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TABLE  B-VI 
SUBROUTINE  DENS 


C 

C 

C 


C 

C 

C 


SUBROUTINE  CENS 

CCMMCN  /FCCM/CCORC,CLAT  ,  PRNT 

R  E  A  L  *8  HKM  F  T  ,RHC,C(jCRC»DLAT  ,PRNT 

C I M  ENS  I  CN  H TKM  (  1  CO), RHCM (  100) , HTKFT(  IOC)  ,RHCE(  ICO) 
SET  UP  ALTITLCE  ARRAY  HTKM,  IN  KILCMETERS 
CC  3  -1  =  1,51 

H  T  KM  (  I  )=2.0*FCCAT(  I-  1  ) 

3  CONTINUE 

CC  A  J= 1  ,  10 

HTKM(J+51)=110.0+FLCAT(J-1)*10.C 

4  CCNTINUE 


SET  UP  ATMOSPHERIC  CENSITY 

RhCM(  1  )  =  1  *  2 2 50 
RHCM ( 2  )  =  1 .0066 
RHCM ( 3  )  =  8.  1935E-  1 
RHCM ( 4 ) =6,601  1  E  —  1 
RHCM (5  )  =  5.2579E-1 
HE  CM  (6  )  =  4.  135U-1 
RhCM (  7  )  =3.  1  194E-  1 
RHCM ( 8 ) =2 • 2  786  L- 1 
RHCM ( 9  )  =  1  • 66 A 7E- I 
RHCM (  10  )  =  1 .2165E-  1 
RHCM (  11  )=8.8910E-2 
RhCM (  12 ) =6.45  ICE- 2 
RHCM (13) =4.693BE-2 
RHC  M (  14 ) =3.4257E-2 
R  H  C  M (  l6)=2.5076E-2 
RHCM (  16 )  =  1 .841CE-2 
RHCM (  1  7  )=1.3555E-2 
RHCM(  18  )=9.88  74L-3 
RHC M  (  i9)=7.2579t-3 
RhCM ( 20  )  =5 -  3666r- 3 
RHCM( 21  )  =3.  ^  9  6  7  L- 3 
RHCM (22  )  =2.9 948E- 3 
RhCM(2J)=2.258SE-J 
RHCM ( 24  )  =  1 . 7141L-3 
RHCM(2b)=l.3l67h-J 
RHCM (26)  =  1.026 9 E“3 
RHCM ( 27)=8.CC9/t-4 
R  H  C  M ( 2  8  )=6.3137L-4 
RHCM ( 29 ) =4.9  7  6 2l- 4 
RHCM ( 30 )  =  3.9G86L-4 
RHCM ( 3  1  )=3.0592E-4 
R  H  C  M ( 32)=2.3931E-4 
RHCM(3J)=1.8837t-4 
RHCM( 34)=1. 47131-4 
RhCM (  35  )  =  1  .  1399L-4 
RHCM ( 36 ) =8. 7535E-5 
RHC  M ( 3  7  )=6.6593E-5 
RHCM ( 38  )  =  5.0  15  lt-5 
RHCN(39)=3.736E-5 
RHCM ( 4C  )=2.750L-5 
RHCM ( 4  I  )  =  1 . 9S9E-5 
RHCM(42  )  =  1.382E-5 
RhCM(43)=9.563h-6 
RHCM ( 44  )  =6.6  17  L  —  fc 


ARRAY  RHCM,  IN  KG/METER**  3 
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RHCM ( 45 )=4*579E-6 
RHCM ( 46 )  =  3  • 170 E- 6 
RHCMI47 )  =  2, 137E-6 
RHCM(48)=1.459E-6 
RHCM(49)=1«008E— 6 
R  HCM ( 50 )=7*044E— 7 
RHCM(5l)=4.974E-7 
RHCM(52)=9.829E-8 
RHCM<53)=2.436E-8 
RHCM(54 )=7.589E-9 
RHCM  (  55  )  -  3. 3  94  E— 9 
RH  CM ( 56 )=1.836E-9 
RHCM (57 )  =  1  • 1 59  E-9 
RHCM ( 58 ) =8«036E- 10 
RHCM( 59 )=5.858E— 10 
RHCM (60) =  4  . 34  7  E* 10 
RHCM(6l )=3.318E*10 
C 

C  SET  UP  ALTITUDE  ARRAY  HTKFT,  IN  KPT 

C 

CC  5  1=1,51 

HTKFT  (  I  )  =  6  «0  *  FLOAT  (  1*1) 

5  CCNTINUE 

CO  6  J= 1,  10 

HTKFT  (J+51)=33G*C+FLCAT  (J-l)*30.0 

6  CCNTINUE 
C 

C  SET  UP  ATMOSPHERIC  DENSITY  ARRAY, RHOE,  IN 

C 

RHCE f 1  )=7.6474E-2 
RHCE ( 2 )  =6  . 39 25 E- 2 
RHCE(3)=5*3022E-2 
RHCE ( 4  )  =  4  . 3606  E* 2 
RHCE (5 )=3.5531E-2 
RHCE (6)=2.8657E-2 
RHCE ( 7 )=2. 2853E-2 
RHCk(8)=l«7 170E-2 
RHCE ( 9 )  =  1 • 2884  E-2 
RHC  E ( 10>=9.6701E-3 
R  HCE (  1  1  )  =  7.2589E-3 
RHC  E (  1 2 ) =  5 . 4485E-3 
RHCE (  13)=4.0624E-3 
RH  CE (  14 )  =  3 . 03 6 8 E- 3 
R  HCE ( 15 ) =2 . 2759E— 3 
RHCE ( 16 )  =  1 • 7 10CE—  3 
R  HCE ( 17 )  =  1 • 2879E- 3 
RHCE  (  18  )  =  9 • 7 2 A  IE-4 
RHCE ( 19 ) =7  «  3 18  8E-4 
RHCE ( 20 )=5.4944E-4 
RHCE ( 21 ) =4.5106E-4 
RHCE(22)=3. 1543E-4 
RHCE(23)=2.411CE-4 
RHCE(24)=1.853CE*4 
RHC  E ( 25 )  =  1  •  4  3  1  7E~  4 
RHCE (26  )  =  1 • 1 119 E* A 
RHCE ( 27 ) =  8.6943E-*5 
RHCE(28)=6.9261E-5 
RHCE ( 29 )=5.5183E*5 
RHCE ( 30 )=4.4159E-5 
R  HCE ( 31 )  =  3. 5578E-5 


LBS/FT**3 
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RHCE! 32)=2.8583E-5 
RHCE! 33)=2.2898E-5 
RHCE(34)*1.8289E-5 
RHCE { 35 )  - 1 *4627E-5 
RHCE (36U1. 1748E-5 
RHCE { 37 )-9*  3752E-6 
RHCE { 38)=7*4307E-6 
RHCE  <  39 ) =  5 • 8469E-6 
RHCEI40)=4*5fc53E-6 
RHCE {41  )=3.5353E— 6 
RHCE (42 )-2*7 14E-6 
RHCE {43)=2.063E-6 
RHCE(44)=1.553E-6 
RHCE {45 )= 1 . 1 4  5  E-6 
RHCE ( 46  )=8.  172E-7 
RHCE (47 )=5.835E-7 
RhCE ( 4  8  )  =  4  . 166 E—  7 
RHCE (49  )  =  2.976E-7 
RHCE ( 50 ) =2  • 126 E- 7 
RHCE  (51  )=1.488E— 7 
PHCE<52)*2.796E-8 
RHCE {53)=6.385E-9 
RHCE ( b A ) = 1 • 743E-9 
RhCE  (  5  5 )  =5.751E-1C 
RhCE(5G)=2.6C0E-lC 
RHCE (57)=1*415E— 10 
RHCE ( 58  )  =  8 .84  5E-  1 1 
RhCE (59)=6.C90t-ll 
RHCE  {  cO  )  =  4.448E— 1  1 
RHCE (61  ) =3. 350 E— 11 
RETURN 
C 

ENTRY  AT  N ( HKN  F  T  »RHC) 

C 

R  HC  =  0  .  G 

IF  (hKNE  T  )  13,  IOC,  ICO 
100  IF (CCCRC)  1,2,2 
C 

C  CATA  IN  NETRIC  UNITS 

C 

1  IF(HKMFT-1.D2>11, 11,15 

11  L=HKMFT/2.D0+1.D0 

12  A* ( HKMFT-HTKM ( L ) ) / ( HTKM ( L ♦ 1 ) -HTKM ( L ) ) 

14  RHORHOM  (  L  )  *  (  RHOM  ( L ♦ 1 ) /RHOM ( L ) )  **A 

RETURN 

15  IF(HKMFT-2.D2)17,18,16 

17  L~5 1*00000 1+ ( HKMFT— 1«02 )/ 1*01 
GO  TO  12 

18  RHOaRHOM (61) 

RETURN 

C 

C  DATA  IN  ENGLISH  UNITS 

C 

2  IF(HKMFT-3.D2)21?21?25 

21  L-HKMFT/6.D0+1. 0000 001 

22  A=(HKMFT-HTKFT(L) )/(HTKFT(L+l )-HTKFT ( L ) ) 
24  RHO=RHOE ( L ) * ( RHOE( L* 1 ) /RHOE ( L ) ) **A 

RETURN 

25  IF(HKMFT-6.D2)26,27, 16 

26  L=51.000001*(HKMFT-3.D2)/3.D1 
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GC  TC  22 
27  RHC  =  RHCE ( 6 i  ) 

16  RETURN 
13  RRlTE(6f13C) 

130  F  C  RM  A  T ( /  5  X » 1  * ♦ *♦*  EARTH  IMPACT  ****♦•) 
RETURN 
ENC 


TABLE  B-VII 
SUBROUTINE  XYTOR 


SUBROUT  I NE  XYTOR ( X»Y»Z » XDOT , YDOT , l DO T , R f A , E , ROOT , ADOT , EDO T ) 
C 

C  SUBROUTINE  TC  CONVERT  FROM  XYZ  COORDINATES  TO  RADAR  POLAR 

C  COORDINATES 

C 

RE  AL  *8  X*YtZ»XDUT ,  YOOT , ZDOT , R , A , E , ROOT , ADOT f EDOT 
IMPLICIT  REAL*H( A-HfO-Z) 

R=DSQRT(X**2+Y**2+Z**2) 

E  =  CAR  S I N ( Z/ R  ) 

A=DATAN2(X,Y) 

RDOT= ( X  +  XDOT  +Y*YDOT+Z*ZDOT ) /R 
ADCT  =  ( Y*XOCT-X*YDGT ) / ( X**2* Y**2 ) 

CENOM=  DA  BS (R**2-Z**2) 

EDO  T= ( R*ZDCT-Z*KDGT ) / ( DSORT ( DENOM ) *R ) 

RETURN 

END 


TABLE  B- VIII 
SUBROUTINE  RTOXY 


SUBROUTINE  R TOXY ( R , A , E , RD01 , ADO T f E DO T f X , Y f Z , XDUT , YDOT » ZDO T ) 

C 

C  SUBROUTINE  TC  CONVERT  FROM  RADAR  COORDINATES  TO  XYZ  COORDINATES 

C 

RE  AL  *8  K»A»EfRCOT»  ADO  T *  E CUT  f  X  »  Y  t  Z  » XDOT , YDO T , ZDO T 
IMPLICIT  RE AL  *8 ( A-H,0-Z ) 

SAZ-DSINI A) 

SEL-DS I N ( E ) 

C A Z  =  DCU S ( A  ) 

C £  L=  DCC  S ( E  ) 

X=R*CEL*SAZ 
Y=R*CEL*CAZ 
Z=K*SCL 
RPR  =  ROC  T /R 

XDCT=KPK*X-LC0T*Z*SAZ+ADCT*Y 

YDCT=RPR*Y-tUOT*Z*CAZ-ADOT*X 

ZDCT=KPR*Z+ECOT*R*CLL 

RETURN 

ENC 
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TABLE  B-IX 
SUBROUTINE  XCOVTR 


SUBROUTINE  XCOVTR ( XCOV, XVCTR, RCOV , NCODE I 
RE AL*8  XCOV, XVCTR, RCOV 
IMPLICIT  REAL*8(A-H,0-Z ) 

0 1  MENS  I  ON  X VCTR ( 7 ) 

DIMENSION  XCOV (7,7) , RCOVC 7, 7 ) ,RVCTR( 7) 

DIMENSION  C0NVT(7, 7),DUMI(7,7) 

DIMENSION  CONV ( 7 , 7 ) 

X  - XVC  T  R (  1  ) 

Y  =  X  VC  TR ( 2 ) 

Z  =X VCTR ( 3 ) 

XP=XVCTR(4) 

YP  =  X VC  TR ( 5 ) 

ZP=X VC TR(6) 

RVCTR(7)=XVCTR(7) 

CALL  XYTOR(X, Y , Z , X P , YP ,Z P , RVCTR ( 1 ) , RVCTR( 2 ) , RVCTR ( 3 ) , R VCTR ( 4 ) ,RVCT 
IR( 5)  , RVC  T  R ( 6 )  ) 

RVSQ  =  R VC  TR ( I ) **2 
RSC=X**2+Y**2 


RX Y=  DSQRT ( RSQ  ) 

00  I  J=  1 , 7 
DO  I  K=  1 , 7 
RCOV ( J , K ) =0 • 

1  CONVt.J,  K)  =  0. 

CONV( 1, 1 ) -X/ RVCTR ( 1 ) 

CONV( 1 ,2 )=Y/RVCTR( 1 ) 

CONV ( 2 , 1 ) = Y/RSQ 
C0NV(2,2)=-X/RSQ 
CONV( 1, 3)=Z/RVCTR( I) 

CONV ( 3 , I )=-X*Z/(RXY*RVSQ) 

C0NV(3,2 )=-Y*Z/(RXY*RVSU) 

CONV (3,3)=  RXY/RVSQ 

CON V ( 4 , 1 )=( RVCTR( I ) ♦XP-RVCTR ( 4 ) *X ) /RVSQ 
C0NV(4,2 )  =  ( R VC  TR ( 1 ) ♦YP-RVC T R ( 4 ) *Y ) /R VSQ 
C0NV(4, 3 )= (RVCTR( I ) *Z P- RVC TR ( 4 ) *Z ) /R VSQ 
C0NV(5,I)=-(2«00*X*RVCTR(5)+YP)/RSQ 
CONV(5,2)S(XP-2«00*YJ*RVCTR( 5) )/RSQ 

CONV (6, I)*-X*RVCTR(6)/RSQ-2.00»RVCTR(4)*CONV(3,I ) /RVCTR (I )-Z*XP7(R 
1 VS  Q*RX Y ) 

C0NV(6,2)»-Y*RVCTR(6)/RSQ-2.00^RVCTR(4)*C0NV(3,2)/RVCTR( I )-Z*YP/(R 
1  VSQ*RX Y ) 

CGNV(6,3)=-Z*RVCTR(6)/RVSQ-RVCTR(4)*C0NV( 3, 3) /RVCTR ( I ) 

CON V ( 7 , 7 ) *  I • 

DO  2  J=4 , 6 
DO  2  K=4 , 6 

2  CONV(J,K) =CONV ( J-3, K-3 ) 

IF (NCODE) 100,20,20 

20  CONTINUE 
DO  3  J= 1 , 7 
CO  3  K=  1 , 7 

3  CGNVT (J,K)=CCNV(K,J) 

CALL  DMTMUL(XCCV,CONVT ,DUMI,7, 7, 7) 

CALL  DMTMUL(CUNV,DUMl,RCOV,  7,7,7) 

RETURN 

100  CONTINUE 

DO  101  J  =  1 , 7 
DO  101  K= 1 , 7 

101  RCCV( J,K)=CONV( J,K) 

RETURN 

END 
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TABLE  B-X 

ROUTINE  RCOVTX 


SUBROUTINE  RCOVTX < COVR, XSTATE , COVX ) 

REAL*8  COVR, ESTATE, XSTATE, COVX 
IMPLICIT  REAL*8(A-H»0-Z) 

DIMENSION  COVR (7, 7) ,C0VX<7,7) , ESTATE! 7) ,DXDR(7, 7) ,DUMMY(7,7) 
DIMENSION  XSTATE ( 7 ) 

CALL  XYTOR (XSTATE* 1 ), XSTATE *  2 ), XSTATE ( 3 >, XSTATE ( 4 ) , XSTATE (5) ,XSTAT 
1 E ( 6 ) , EST  ATE ( 1 ), ESTATE ( 2 ), E STATE ( 3 ), E STATE ( 4 ), E STATE ( 5 ), ESTATE ( 6 )  ) 
ESTATE (7)=XSTATE ( 7) 

DO  200  J  =  1  *  7 
DO  200  K= 1 , 7 
200  DXDR ( J , K ) =0 • 

SAZ=DSIN(ESTATE*?> 1 
CAZ=DCOS (ESTATE* 2 ) ) 

SEL  =  DSIN(ESTATE<  3) ) 

CEL-DCOS*ESTATE* 3) ) 

C 

C  CONVERT  COVARIANCE  MATRIX  TO  XYZ  COORDINATES 

DXDR ( 1,1 ) =CEL*SAZ 
DXDR ( 1,2)=ESTATE( i)*CEL*CAZ 
DXDR( 1,3)=- ESTATE* 1)*SEL*SAZ 
DXDR ( 2 , 1 ) =CEL*CAZ 
DXDR*2,2)=-ESTATE* 1 )*CEL*SAZ 
DXDR (2 ,  3  )=- ESTATE*  1) ♦SEL*CAZ 
DXDR*3,1)=SEL 
DXDR ( 3 , 2 ) ~0 . 

DXDR(3,3)=ESTATE( 1 )*CEL 

DXDR (4, 1 )=E STATE*  5 ) *CEL^CAZ-ESTATE ( 6 )*SEL*SAZ 

D XDR* 4,2 )  =  ES TAT E ( 4 ) *CEL*CAZ-E STATE* 6) ♦ESTATE* 1 )^SEL*CAZ~E STATE *5)^ 
1  ESTATE  * 1)*CEL*SAZ 

DXDR*4,3)--ESIATE*4)*SEL*SAZ-ESTATE*6)*ESTATE*  1  )*CEL^S AZ-E STATE  *  5 ) 
RESTATE*  1  )*SEL*CAZ 

DXDR *5,1 )=- ESTATE  *  6)*S€L*CAZ-ESTATE( 5)*CEL*SAZ 

DXDR  *  5 ,2 )  =-ES  T  ATE  *  4 )*CEL*SAZ  +  ESTATE  *  6 ) ♦ESTATE  *  1)*SEL*SAZ- ESTATE *5) 
1 *E S T ATfc  *  1 ) ♦CEL^CAZ 

DXDR*  5, 3  )=-E STATE (4 ) ♦SEL^CAZ-ESTATE *  6 ) ♦ES TATE  * 1)*CEL*CAZ  +  ESTATE(5) 
l*ESTATb*  1)*SEL*SAZ 
DXDR  *6, 1 )  =  ESTATE(6)*CEL 
DXDR ( 6 , 2  )  =  0 • 

DXDR  *6,3)  =  E STATE  *4 )*CEL~E ST ATE*  6)*E STATE* 1 )*SEL 
CO  300  J  =  4,6 
DO  300  K=4 , 6 

300  DXDR* J,K)=DXDR* J-3,K-3> 

CXDR*  7, 7)  =  1. 

CO  301  J= 1 , 7 
DO  301  K  =  l,7 
DUMMY*  J,K)=0. 

DO  301  L= 1 , 7 

301  DUMMY*  J,K)  =  DUMMY* J,K)+COVR* J,L)*DXDR*  K,L) 

DO  302  J=l,7 

DO  302  K=l,7 
COVX* J,K)=0. 

DO  302  L=l,7 

302  COVX* J,K)=CGVX* J,K)+DXDR* J,L) ♦DUMMY* L,K) 

RETURN 

END 
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TABLE  B-XI 
SUBROUTINE  GAUSSN 


SUBROUTINE  GAUSSN { NS , RN , S I GMA ) 
R EAL *8  RN, SIGMA 
C I  MENS  I  ON  R N {  1  ) 

DC  9  J= I , NS 
3  V=R AN ( X ) 

V=- ALUG ( V ) 

V  =  R  A  N  {  X  ) 

Z=-ALOG{ V) 

IF {  { Y-l.  )**2-2.*Z  )6,6, 3 

6  S  =  R  A  N  {  X  ) 

l  F { S— • 5 ) 7 , 6 , 8 

7  RN{ J)=Y+SIGMA 
GC  TO  9 

8  RN { J  )=-Y*S I  GMA 

9  CONTINUE 
RETURN 
ENC 


TABLE  B-XII 
SUBROUTINE  DMTMUL 


SUBROUTINE  DMTMUL { A , B , AB , NR A , NC A , NCB ) 
DOUBLE  PRECISION  A  ,  B  »  AB 
DIMENSION  A(7f7)fB(7f7)»A8(7f7) 

DC  I  J  =  1 1  N R A 
DC  1  K= 1 » NCB 
A  B ( J , K )  =  0  . 

DC  I  L=1,NCA 

I  AB(J»K)  =  AB(JfK)+A{JfL ) *  B { L  »  K ) 

RETURN 

END 
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MI  NV 
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MI  NV 

028 

c 
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MINV 

029 
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MINV 

030 

c 

•  MINV 

031 

c 

MINV 

032 

SUBROUTINE  MINV ( A,N, 0,L ,M ) 

MI  NV 

033 
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MI  NV 

034 

c 

MINV 

035 

c 

.MINV 

036 

c 

MINV 

037 

c 

IF  A  CCU8LE  PRECISION  VERSION  OF  THIS  ROUTINE  IS  DESIREO,  THE 

MI  NV 

038 

c 

C  IN  COLUMN  1  SHOULD  BE  REMOVED  FROM  THE  DOUBLE  PRECISION 

MINV 

039 

c 

STATEMENT  WHICH  FOLLOWS. 

MINV 

040 

c 

MI  NV 

04  1 

DOUBLE  PRECISION  A , D , B I GA , HOL 0 

MINV 

042 

c 

MINV 

043 

c 

THE  C  MUST  ALSO  BE  REMOVED  FROM  DOUBLE  PRECISION  STATEMENTS 

MI  NV 

044 

c 

APPEARING  IN  OTHER  ROUTINES  USED  IN  CONJUNCTION  WITH  THIS 

MINV 

045 

c 

ROUTINE. 

MINV 

046 

c 

MI  NV 

047 

c 

THE  DOUBLE  PRECISION  VERSION  OF  THIS  SUBROUTINE  MUST  ALSO 

MINV 

048 

c 

CONTAIN  DOUBLE  PRECISION  FORTRAN  FUNCTIONS.  ABS  IN  STATEMENT 

MINV 

049 

c 

10  MUST  BE  CHANGED  TO  0A8S. 

MINV 

050 

c 

MINV 

051 

c 

.MINV 

052 

c 

MINV 

053 

c 

SEARCH  FOR  LARGEST  ELEMENT 

MINV 

054 

c 

MI  NV 

055 

D=  1 . 0 

MINV 

056 
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MINV 
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MI  NV 
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K  K  =  NK  E  K 

MI  NV 

062 
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MINV 

063 

CC  20  J=K , N 

MI  NV 
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MI  NV 

065 
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MINV 

066 
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MINV 

067 

1C 

IF (DABSt  BIGA)-CABS( A( IJ ) ) ) 

15,20,20 

MI  NV 

068 

15 

B I G  A  =  A ( I J) 

MINV 

069 
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MI  NV 

070 

K  (  K  )  =  J 

MI  NV 

071 

20 

CONTINUE 

MINV 

072 

c 

MINV 

073 
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INTERCHANGE  RCHS 

MI  NV 

07  A 

c 

MINV 

075 

J=L ( K ) 

MINV 

076 

IF ( J —  K )  35,35,25 

MINV 

077 

25 

K  I  =  K  -  N 

MINV 

078 

CC  30  1*1, N 

MINV 

079 
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MINV 

080 
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MINV 

081 
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MINV 

082 

A  (  K 1  )  =M  J I  ) 

MINV 

083 

30 

A ( J I )  =HCLC 

MINV 

08  A 

c 

MINV 

085 

c 

INTERCHANGE  COLUMNS 

MINV 

086 

c 

MINV 

087 

35 
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MINV 

088 
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MINV 

089 

38 
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MINV 

090 
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091 
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MINV 
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MINV 
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100 
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MI  NV 

101 

A  6 
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MINV 
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MINV 

1 0  A 

IF(I-K)  50,55,50 

MINV 

105 

50 

I K  =  NK  L I 

MI  NV 

106 
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MINV 

107 

55 
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MINV 

108 

c 
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109 

c 
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I K  =  NK  L I 

MINV 

113 

I J= I — N 

MINV 

1 1  A 

CC  65  J  =  1  ,  N 

MI  NV 

115 

I  J=  I  JEN 

MINV 

116 

1 F (  I  —  K )  60,65,60 

MINV 

117 

60 

IF(J-K)  62,65,62 

MI  NV 

118 

62 

K J=I J-16K 

MINV 

119 

A (  I  J  )  =  A (  IK)*A(KJ)6A(  IJ) 

MINV 

120 

65 

CONTINUE 

MINV 

121 

79 


TABLE  B-XIII  (Continued) 

c 
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MINV 
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